windalib.s

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

/*******************************************************************************
*
* reschedule - 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 are
* accurately stored in the task control blocks when entering this function.
* The status register is 0x3000. (Supervisor, Master Stack, Interrupts UNLOCKED)
*
* The register a0 must contain the value of _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 ()

*/

_reschedule:
	movel	_readyQHead,d0			/* get highest task to d0 */
	jeq 	idle				/* idle if nobody ready */

switchTasks:
	movel	d0,_taskIdCurrent		/* update taskIdCurrent */
	movel	a0,a1				/* a1 gets previous task */
	movel	d0,a0				/* a0 gets highest task*/

	movew	a0@(WIND_TCB_SWAP_IN),d3	/* swap hook mask into d3 */
	movew	a1@(WIND_TCB_SWAP_OUT),d0	/* get swap out hook mask */
	orl	d0,d3				/* or masks together */
	andl	#0xffff,d3			/* clear out upper bits */
	jne 	doSwapHooks			/* any swap hooks to do */
	tstl	_taskSwitchTable		/* any global switch hooks? */
	jne 	doSwitchHooks			/* any switch hooks to do */

dispatch:
	movel	a0@(WIND_TCB_ERRNO),d0		/* restore errno */
	movel	d0,_errno
	movel	a0@(WIND_TCB_SSP),a7		/* push dummy except */

	movew	_intLockIntSR,d1		/* LOCK INTERRUPTS */
	movew	d1,sr

	tstl	__coldfireHasMac
	jeq 	dispatchNoMac			/* jump if no MAC */

	movel	a0@(WIND_TCB_MAC),d0		/* restore MAC */
	.word	0xa100				/* movel d0,acc */
	movew	a0@(WIND_TCB_MACSR),d0		/* restore MACSR */
	.word	0xa900				/* movel d0,macsr */
	movew	a0@(WIND_TCB_MASK),d0		/* restore MASK */
	.word	0xad00				/* movel d0,mask */

dispatchNoMac:

	moveml	a0@(WIND_TCB_REGS),d0-d7/a0-a6	/* load register set */
	
	tstl	_workQIsEmpty			/* if work q is not empty */
	jeq 	doWorkUnlock			/* then unlock and do work */

#ifdef WV_INSTRUMENTATION	/* wv2: reschedule (work queue empty) */

	/* Is instrumentation on?
	*/
	tstl	_evtAction
        jeq     wv2_skip

	/* Save registers.
	*/
	subl	#24,a7
	moveml  d0-d3/a0-a1,a7@	

        /* Do we need to log this event?
	*/
        movel   _wvEvtClass,d2
        andl    #WV_CLASS_1_ON,d2
        cmpl    #WV_CLASS_1_ON,d2
        jne     wv2_chkTrig
 
        movel   _taskIdCurrent, a0		/* current task */
        movel   a0@(WIND_TCB_PRIORITY),d3	/* ...and priority  */

        /* Is the task running with an inherited priority? If so,
	 * a different event is logged.
	*/
        movel   #EVENT_WIND_EXIT_DISPATCH,d1
        cmpl    a0@(WIND_TCB_PRI_NORMAL), d3
        jge     wv2_log
        movel   #EVENT_WIND_EXIT_DISPATCH_PI, d1

wv2_log:
        /* Now log the event.
	*/
        movel   d3,a7@-
        movel   a0,a7@-
        movel   d1,a7@-
        movel   __func_evtLogTSched,a1
        jsr     a1@
        addl    #12,a7

wv2_chkTrig:
        /* Do we need to evaluate triggers for this event?
	*/
        movel   _trgEvtClass,d2
        andl    #TRG_CLASS_1_ON,d2
        cmpl    #TRG_CLASS_1_ON,d2
        jne     wv2_restore

        movel   _taskIdCurrent,a0		/* current task */
        movel   a0@(WIND_TCB_PRIORITY),d3	/* ...and priority  */

        /* Is the task running with an inherited priority? If so,
	 * a different event is logged.
	*/
        movel   #EVENT_WIND_EXIT_DISPATCH,d1	/* event ID is now in d1 */
        cmpl    a0@(WIND_TCB_PRI_NORMAL),d3
        jge     wv2_trigger
        movel   #EVENT_WIND_EXIT_DISPATCH_PI, d1

wv2_trigger:
        /* Evaluate triggers.
	*/
        clrl    a7@-				/* param 8 - unused */
        clrl    a7@-				/* param 7 - unused */
        clrl    a7@-				/* param 6 - unused */
        movel   d3,a7@-				/* param 5 - priority */
        movel   a0,a7@-				/* param 4 - task id */
        clrl    a7@-				/* param 3 - unused */
        movel   #TRG_CLASS1_INDEX,a7@-		/* param 2 - class index */
        movel   d1,a7@-				/* param 1 - event id */
        movel   __func_trgCheck,a0
        jsr     a0@
        addl    #32,a7				/* Pop params */

wv2_restore:
	moveml  a7@,d0-d3/a0-a1			/* restore regs */
	addl	#24,a7
wv2_skip:

#endif	/* WV_INSTRUMENTATION wv2 */

/*
 * At this point we are dispatching to the current
 * task.  The last thing to do is build the exception frame and
 * we need to restore proper format and PC.  Format
 * and PC are stored in the TCB, so we need to get taskIdCurrent again.  All
 * the registers have already been restored so we need to
 * get a working register by saving to stack.
 */
        lea     -16(a7),a7                      /* create stack frame + 8 for reg */
        movel   a0,a7@                          /* save a0 at top of stack */
        movel   d0,a7@(4)                       /* save d0 at top of stack */
        movel   _taskIdCurrent,a0               /* get current task control block */
        movel   a0@(WIND_TCB_HASMAC),a7@(8)     /* onto stack */
        movel   a0@(WIND_TCB_PC),a7@(12)        /* push new pc and proc status */
        movel   #0x40003000,d0                  /* TCB value set back to default */
        movel   d0,a0@(WIND_TCB_HASMAC)         /* TCB value set back to default */
                                                /* if an exception occurs, HASMAC will */
                                                /* have the proper value from instr above */
        movel   a7@+,a0                         /* restore a0 */
        movel   a7@+,d0                         /* restore d0 */
        clrl    _kernelState                    /* release kernel mutex */
        rte                                     /* UNLOCK INTERRUPTS */

/*******************************************************************************
*
* idle - spin here until there is more work to do
*
* When the kernel is idle, we spin here continually checking for work to do.
*/

idle:

#ifdef WV_INSTRUMENTATION	/* wv3: idle loop entry */

	/* Is instrumentation on?
	*/
	tstl	_evtAction
        jeq     wv3_skip

	/* Save registers.
	*/
	subl	#24,a7
	moveml  d0-d3/a0-a1,a7@	

        /* Do we need to log this event?
	*/
        movel   _wvEvtClass,d2
        andl    #WV_CLASS_1_ON,d2
        cmpl    #WV_CLASS_1_ON,d2
        jne     wv3_chkTrig
 
        /* Now log the event. Interrupts need to be disabled.
	*/

      	movew   sr,d2
        movel   d2,a7@-
        movew	_intLockIntSR,d2		/* LOCK INTERRUPTS */
	movew	d2,sr

        movel   #EVENT_WIND_EXIT_IDLE,a7@-
        movel   __func_evtLogT0_noInt,a1
        jsr     a1@
        addl    #4,a7           /* correct stack */
        
        movel   a7@+,d2         /* get SR back */
        move    d2,sr
        
wv3_chkTrig:
        /* Do we need to evaluate triggers for this event?
	*/
        movel   _trgEvtClass,d2
        andl    #TRG_CLASS_1_ON,d2
        cmpl    #TRG_CLASS_1_ON,d2
        jne     wv3_restore

wv3_trigger:
        /* Evaluate triggers.
	*/
        clrl    a7@-				/* param 8 - unused */
        clrl    a7@-				/* param 7 - unused */
        clrl    a7@-				/* param 6 - unused */
        clrl	a7@-				/* param 5 - unused */
        clrl	a7@-				/* param 4 - unused */
        clrl    a7@-				/* param 3 - unused */
        movel   #TRG_CLASS1_INDEX,a7@-		/* param 2 - class index */
        movel   #EVENT_WIND_EXIT_IDLE,a7@-	/* param 1 - event id */
        movel   __func_trgCheck,a0
        jsr     a0@
        addl    #32,a7				/* Pop params */

wv3_restore:
	moveml  a7@,d0-d3/a0-a1			/* restore regs */
	addl	#24,a7
wv3_skip:

#endif	/* WV_INSTRUMENTATION wv3 */

	movew	#0x3000,sr			/* UNLOCK INTERRUPTS (in case)*/

	movel	#1,d0				/* set idle flag for spyLib */
	movel	d0,_kernelIsIdle
idleLoop:
	jsr	idlejsr
	tstl	_workQIsEmpty			/* if work queue is empty */
	jne 	idleLoop			/* keep hanging around */
	clrl	_kernelIsIdle			/* unset idle flag for spyLib */
	jra 	doWork				/* go do the work */
/*
 * this looks funny, but is necessary to allow the visionTrace
 * to work with the ColdFire processor.
 */
idlejsr:
	nop
	nop
	rts

/*******************************************************************************
*
* doSwapHooks - execute the tasks' swap hooks
*/

doSwapHooks:
	pea	a0@			/* push pointer to new tcb */
	pea	a1@			/* push pointer to old tcb */
	lea	_taskSwapTable,a5	/* get adrs of task switch rtn list */
	moveq	#-4,d2			/* start index at -1, heh heh */
	jra 	doSwapShift		/* jump into the loop */

doSwapHook:
	movel	a5@(0,d2:l),a1		/* get task switch rtn into a1 */
	jsr	a1@			/* call routine */

doSwapShift:
	addql	#4,d2			/* bump swap table index */
	lsll	#1,d3			/* shift swapMask bit pattern left */
	btstl	#16,d3			/* check if bit shifted out of lsw */
	jne 	doSwapHook		/* if bit set then do ix hook */
	tstw	d3			/* check if lower 16 bits are zero */
	jne 	doSwapShift		/* any bits still set */
					/* no need to clean stack */
	movel	_taskIdCurrent,a0	/* restore a0 with taskIdCurrent */
	tstl	_taskSwitchTable	/* any global switch hooks? */
	jeq 	dispatch		/* if no then dispatch taskIdCurrent */
	jra 	doSwitchFromSwap	/* do switch routines from swap */

/*******************************************************************************
*
* doSwitchHooks - execute the global switch hooks
*/

doSwitchHooks:
	pea	a0@			/* push pointer to new tcb */
	pea	a1@			/* push pointer to old tcb */

doSwitchFromSwap:
	lea	_taskSwitchTable,a5	/* get adrs of task switch rtn list */
	movel	a5@,a1			/* get task switch rtn into a1 */

doSwitchHook:
	jsr	a1@			/* call routine */
	addql	#4,a5			/* bump to next task switch routine */
	movel	a5@,a1			/* get next task switch rtn */
	cmpl	#0,a1			/* check for end of table (NULL) */
	jne 	doSwitchHook		/* loop */
					/* no need to clean stack */
	movel	_taskIdCurrent,a0	/* restore a0 with taskIdCurrent */
	jra	dispatch		/* dispatch task */

/*******************************************************************************
*
* doWork - empty the work queue
* doWorkUnlock - unlock interrupts and empty the work queue
*/

doWorkUnlock:
	movew	#0x3000,sr		/* UNLOCK INTERRUPTS */
doWork:
	jsr	_workQDoWork		/* empty the work queue */
	movel	_taskIdCurrent,a0	/* put taskIdCurrent into a0 */
	movel	_readyQHead,d0		/* get highest task to d0 */
	jeq	idle			/* nobody is ready so spin */
	cmpl	a0,d0			/* compare to last task */
	jeq	dispatch		/* if the same dispatch */
	jra	switchTasks		/* not same, do switch */

#endif	/* !PORTABLE */

#ifdef PORTABLE

/*******************************************************************************
*
* windLoadContext - load the register context from the control block
*
* The registers of the current executing task, (the one reschedule chose),
* are restored from the control block.  Then the appropriate exception frame
* for the architecture being used is constructed.  To unlock interrupts and
* enter the new context we simply use the instruction rte.
*
* NOMANUAL

* void windLoadContext ()

*/

_windLoadContext:
	movel	_taskIdCurrent,a0		/* current tid */
	movel	a0@(WIND_TCB_ERRNO),d0		/* save errno */
	movel	d0,_errno
	movel	a0@(WIND_TCB_SSP),a7		/* push dummy except. */

	movel	a0@(WIND_TCB_PC),a7@-		/* push new pc and sr */
	movel	a0@(WIND_TCB_HASMAC),a7@-	/* onto stack */
	movel   #0x40003000,d0			/* TCB value set back to default */
	movel   d0,a0@(WIND_TCB_HASMAC)		/* TCB value set back to default */
                                                /* if an exception occurs, HASMAC will */
                                                /* have the proper value from instr above */

	tstl	__coldfireHasMac
	jeq 	windLoadCtxtEnd			/* jump if no MAC */

	movel	a0@(WIND_TCB_MAC),d0		/* restore MAC */
	.word	0xa100				/* movel d0,acc */
	movew	a0@(WIND_TCB_MACSR),d0		/* restore MACSR */
	.word	0xa900				/* movel d0,macsr */
	movew	a0@(WIND_TCB_MASK),d0		/* restore MASK */
	.word	0xad00				/* movel d0,mask */

windLoadCtxtEnd:

	moveml	a0@(WIND_TCB_REGS),d0-d7/a0-a6	/* load register set */
	rte					/* enter task's context. */

#endif	/* PORTABLE */

/*******************************************************************************
*
* intEntTrap - trap entry for interrupts to allow stack switching.
*
* intEntTrap is called by use of a trap #XXX instruction at the start of
* an interrupt service routine (in the intConnect stub).  At the point
* this routine is called, there are two exception stack frames on the
* stack: the exception frame for the interrupt and the exception frame
* for the trap.
* This routine should NEVER be called from C.
*
* SEE ALSO: intConnect(2)

* void intEntTrap ()

*/