windalib.s

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

_intEntTrap:
	movew	#0x3700,sr	/* disable interrupts, turn on M bit */
	/*
	 * stack (task or interrupt):
	 *	trap frame format,fs,vector,sr
	 *	trap frame PC
	 *	interrupt frame format,fs,vector,sr
	 *	interrupt frame PC
	 */
	movel	d0,a7@-			/* save d0 */
	
	/* check SR from interrupt to see if already on int stack. */
	movew	a7@(4+8+2),d0		/* ISR frame SR */
	andl	#0x0700,d0		/* if the interrupt level > 0 */
	beq	0f			/*   was in ISR */
	movel	a7@+,d0			/* restore d0 */
	rte				/* rte to intConnectStub */
0:
	movel	a0,a7@-			/* save a0 */
	/* switch to ISR stack */
	movel	_vxIntStackBase,a0	/* pointer into ISR stack */
	/*
	 * task stack:
	 *	saved a0
	 *	saved d0
	 *	trap frame format,fs,vector,sr
	 *	trap frame PC
	 *	interrupt frame format,fs,vector,sr
	 *	interrupt frame PC
	 */
	/* push pointer to task stack interrupt frame */
	movel	a7,d0			/* ptr to trap frame */
	addl	#16,d0			/* pop trap frame and saved regs */
	movel	d0,a0@-			/* ptr to int frame on ISR stack */
	
	/* copy over interrupt and trap frames */
	movel	a7@(8+8+4),a0@-		/* int frame PC */
	movel	a7@(8+8+0),a0@-		/* int frame fmt,fs,vector,sr */
	movel	a7@(8+4),a0@-		/* trap frame PC address */
	movel	a7@(8+0),a0@-		/* trap frame fmt,fs,vector,sr */
	
	movel	a0,a7@-			/* push new stack pointer */
	movel	a7@(4),a0		/* restore a0 */
	movel	a7@(8),d0		/* restore d0 */
	movel	a7@,a7			/* new SP, switch stacks */
	/*
	 * task stack:
	 *	interrupt frame format,fs,vector,sr
	 *	interrupt frame PC
	 *	
	 * interrupt stack:
	 *	trap frame format,fs,vector,sr
	 *	trap frame PC
	 *	interrupt frame format,fs,vector,sr
	 *	interrupt frame PC
	 *	pointer to interrupt frame on task stack (for intExit).
	 */
	rte				/* rte to intConnect stub */
	
/*******************************************************************************
*
* intEnt - enter an interrupt service routine
*
* intEnt must be called at the entrance of an interrupt service routine.
* This normally happens automatically, from the stub built by intConnect (2).
* This routine should NEVER be called from C.
*
* SEE ALSO: intConnect(2)

* void intEnt ()

*/

_intEnt:
	movel	a7@,a7@-		/* bump return address up a notch */
	movel	d0,a7@-			/* save d0 */
	movel	_errno,d0
	movel	d0,a7@(0x8)		/* save errno where return adress was */
	movel	a7@+,d0			/* restore d0 */
	addql	#1,_intCnt		/* Bump the counter */

#ifdef WV_INSTRUMENTATION	/* wv4: intEnt */

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

	/* Save registers.
	*/
	subl	#44,a7
	moveml  d0-d2/a0-a6,a7@	
	movew	sr,d1
	movew	d1,a7@(40)		/* Previous SR remains in d1 */
	movew	_intLockIntSR,d0
	movew	d0,sr			/* Lock interrupts */

        /* Do we need to log this event?
	*/
        movel   _wvEvtClass,d2
        andl    #WV_CLASS_1_ON,d2
        cmpl    #WV_CLASS_1_ON,d2
        jne     wv4_chkTrig

	/* Calculate event ID from the interrupt number
	*/
	andl	#0x0700,d1
	asrl	#8,d1
	addl	#MIN_INT_ID,d1

        /* Now log the event.
	*/
        movel   d1,a7@-
        movel   __func_evtLogT0_noInt,a1
        jsr     a1@
        addl    #4,a7

wv4_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     wv4_restore

	/* Calculate event ID from the interrupt number
	*/
	movew	a7@(40),d1		/* previous SR */
	andl	#0x0700,d1
	asrl	#8,d1
	addl	#MIN_INT_ID,d1

        /* 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   d1,a7@-				/* param 1 - event id */
        movel   __func_trgCheck,a0
        jsr     a0@
        addl    #32,a7				/* Pop params */

wv4_restore:
	movew	a7@(40),d0
	movew	d0,sr				/* Restore SR */
	moveml  a7@,d0-d2/a0-a6			/* restore regs */
	addl	#44,a7
wv4_skip:

#endif	/* WV_INSTRUMENTATION wv4 */

	rts

/*******************************************************************************
*
* intExit - exit an interrupt service routine
*
* Check the kernel ready queue to determine if rescheduling is necessary.  If
* no higher priority task has been readied, and no kernel work has been queued,
* then we return to the interrupted task.
*
* If rescheduling is necessary, the context of the interrupted task is saved
* in its associated TCB with the PC, SR and SP retrieved from the exception
* frame on the master stack.
*
* This routine must be branched to when exiting an interrupt service routine.
* This normally happens automatically, from the stub built by intConnect (2).
*
* This routine can NEVER be called from C.
*
* It can only be jumped to because a jsr will push a return address on the
* stack.
*
* SEE ALSO: intConnect(2)

* void intExit ()

* INTERNAL
* This routine must preserve all registers up until the context is saved,
* so any registers that are used to check the queues must first be saved on
* the stack.
*
* At the call to reschedule the value of taskIdCurrent must be in a0.
*/


_intExit:
	movel	a7@+,_errno		/* restore errno */
	movel	d0,a7@-			/* push d0 onto interrupt stack */
	movel	a0,a7@-			/* push a0 onto interrupt stack */

#ifdef WV_INSTRUMENTATION	/* wv5: intExit */

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

	/* Save registers.
	*/
	subl	#44,a7
	moveml  d0-d2/a0-a6,a7@	
	movew	sr,d1
	movew	d1,a7@(40)
	movew	_intLockIntSR,d0
	movew	d0,sr

        /* Do we need to log this event?
	*/
        movel   _wvEvtClass,d2
        andl    #WV_CLASS_1_ON,d2
        cmpl    #WV_CLASS_1_ON,d2
        jne     wv5_chkTrig
 
        movel   #EVENT_INT_EXIT, d1		/* event ID is now in d1 */
	tstl	_workQIsEmpty
	jne	wv5_log
        movel   #EVENT_INT_EXIT_K, d1

wv5_log:
        /* Now log the event.
	*/
        movel   d1,a7@-
        movel   __func_evtLogT0_noInt,a1
        jsr     a1@
        addl    #4,a7

wv5_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     wv5_restore

        movel   #EVENT_INT_EXIT, d1		/* event ID is now in d1 */
	tstl	_workQIsEmpty
	jne	wv5_trigger
        movel   #EVENT_INT_EXIT_K, d1

wv5_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   d1,a7@-				/* param 1 - event id */
        movel   __func_trgCheck,a0
        jsr     a0@
        addl    #32,a7				/* Pop params */

wv5_restore:
	movew	a7@(40),d0
	movew	d0,sr				/* Restore SR */
	moveml  a7@,d0-d2/a0-a6			/* restore regs */
	addl	#44,a7
wv5_skip:

#endif	/* WV_INSTRUMENTATION wv5 */

	moveql	#1,d0
	subl	d0,_intCnt		/* decrement intCnt */
	tstl	_kernelState		/* if kernelState == TRUE */
	jne	intRte			/*  then just clean up and rte */

	movew	a7@(10),d0		/* get the sr off the stack into d0 */
	andl	#0x0700,d0		/* if the interrupt level > 0 */
	jne	intRte			/*  then came from ISR...clean up/rte */

	movew	_intLockIntSR,d0	/* LOCK INTERRUPTS */
	movew	d0,sr
	movel	_taskIdCurrent,a0	/* put current task into a0 */
	cmpl	_readyQHead,a0		/* compare to highest ready task */
	jeq	intRte			/* if same then don't reschedule */

	tstl	a0@(WIND_TCB_LOCK_CNT)	/* is task preemption allowed */
	jeq	saveIntContext		/* if yes, then save context */
	tstl	a0@(WIND_TCB_STATUS)	/* is task ready to run */
	jne	saveIntContext		/* if no, then save context */

intRte:
	movew	a7@(10),d0		/* get the sr off the stack into d0 */
	andl	#0x0700,d0		/* if the interrupt level > 0 */
	jne	0f			/* jump if nested interrupt */
	movel	a7@+,a0			/* restore a0 */
	movel	a7@+,d0			/* restore d0 */
 	movel	a7@(8),a7		/* switch back to task stack */
	rte
0:
	movel	a7@+,a0		/* restore a0 */
	movel	a7@+,d0		/* restore d0 */
	rte			/* UNLOCKS INTERRUPT and return to interupted */

/* We come here if we have decided that rescheduling is a distinct possibility.
 * The context must be gathered and stored in the current task's tcb.
 * The stored stack pointer must be modified to clean up the SSP.
 */

saveIntContext:
	/* interrupts are still locked out */
	movel	#1,d0				/* kernelState = TRUE; */
	movel	d0,_kernelState
	movel	_taskIdCurrent,a0		/* tcb to be fixed up */
	movel	a7@+,a0@(WIND_TCB_AREGS)	/* store a0 in tcb */
	movel	a7@+,a0@(WIND_TCB_DREGS)	/* store d0 in tcb */
	movel	a7@(8),a7			/* switch back to task stack */
	movew	#0x3000,sr			/* unlock int., switch to msp */

	/* interrupts unlocked and using master stack*/
	movel	a7@,a0@(WIND_TCB_HASMAC)	/* save sr in tcb */
	movel	a7@(4),a0@(WIND_TCB_PC)		/* save pc in tcb */
	moveml	d1-d7,a0@(WIND_TCB_DREGS4)	/* d1-d7 */
	moveml	a1-a7,a0@(WIND_TCB_AREGS4)	/* a1-a7 */

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

saveIntCtxtMac:
	.word	0xa180				/* movel acc,d0 */
	movel	d0,a0@(WIND_TCB_MAC)		/* save MAC */
	.word	0xa980				/* movel macsr,d0 */
	movew	d0,a0@(WIND_TCB_MACSR)		/* save MACSR */
	.word	0xad80				/* movel mask,d0 */
	movew	d0,a0@(WIND_TCB_MASK)		/* save MASK */

saveIntCtxtMacEnd:
	addl    #8,a0@(WIND_TCB_SSP)            /* adj master stack ptr */
	
	movel	_errno,d0			/* save errno */
	movel	d0,a0@(WIND_TCB_ERRNO)

	jmp	_reschedule			/* goto rescheduler */


/*******************************************************************************
*
* vxTaskEntry - task startup code following spawn
*
* This hunk of code is the initial entry point to every task created via
* the "spawn" routines.  taskCreate(2) has put the true entry point of the
* task into the tcb extension before creating the task,
* and then pushed exactly ten arguments (although the task may use
* fewer) onto the stack.  This code picks up the real entry point and calls it.
* Upon return, the 10 task args are popped, and the result of the main
* routine is passed to "exit" which terminates the task.
* This way of doing things has several purposes.  First a task is easily
* "restartable" via the routine taskRestart(2) since the real
* entry point is available in the tcb extension.  Second, the call to the main
* routine is a normal call including the usual stack clean-up afterwards,
* which means that debugging stack trace facilities will handle the call of
* the main routine properly.
*
* NOMANUAL

* void vxTaskEntry ()

*/

_vxTaskEntry:
	movel	#0,a6			/* make sure frame pointer is 0 */
	movel	_taskIdCurrent,a0 	/* get current task id */
	movel	a0@(WIND_TCB_ENTRY),a0	/* entry point for task is in tcb */
	jsr	a0@			/* call main routine */
	addl	#40,a7			/* pop args to main routine */
	movel	d0,a7@-			/* pass result to exit */
	jsr	_exit			/* gone for good */

/*******************************************************************************
*
* windIntStackSet - set the interrupt stack pointer
*
* This routine sets the interrupt stack pointer to the specified address.
* It is only valid on architectures with an interrupt stack pointer.
*
* NOMANUAL

* void windIntStackSet
*     (
*     char *pBotStack	/@ pointer to bottom of interrupt stack @/
*     )

*/

_windIntStackSet:
	link	a6,#0
	unlk	a6
	rts