semalib.s

vxworks的源代码

	addu	t1, v1			/* point to give rtn */
	lw	t2, 0(t1)		/* get right give rtn for this class */
	j	t2			/* invoke give rtn, it will do rts */
	.end	semTake

/*******************************************************************************
*
* semQGet - unblock a task from the semaphore queue head
*/

	.ent	semQGet
semQGet:
	SETFRAME(semQGet,1)		/* create stack frame */
	subu	sp, FRAMESZ(semQGet)	/* create stack frame */
	SW	ra, FRAMERA(semQGet)(sp) /* save ra */
	li	t1, 1			/* load TRUE */
	sw	t1, kernelState		/* KERNEL ENTER */
#ifdef WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * semGive level 2 (task transition state event )
	 */
        lw      t1, evtAction           /* is WV and-or triggering on? */
        beqz    t1, noSemQGetEvt

	sw	t0, FRAMER0(semQGet)(sp) /* save old sr value */
	SW	a0, FRAMEA0(semQGet)(sp)

        /* we are checking
         * if ((wvEvtClass&(WV_CLASS_2_ON)) != (WV_CLASS_2_ON))
         * leave WV instrumentation and check triggering
         */

        lw      t0, wvEvtClass
        li      t1, WV_CLASS_2_ON
        and     t0, t0, t1
        bne     t1, t0, trgCheckSemQGet

        WV_SETFRAME(semQGetInst,10)
        subu    sp, WV_FRAMESZ(semQGetInst) /* create stack frame */
        sw      ra, WV_FRAMERA(semQGetInst)(sp) /* save ra */

        /* now save the rest of the volatile register set. 
         * This is to ensure that when we come back from the C
         * function callout, all the register are as they were
         * before the call since we don't know what registers
         * the C functions have trashed that the assembly files
         * might rely on.
         */

	/* 
         * no need to save t0 (is saved above) & t1 (gets trashed anyways).
         */

        sw      v0, WV_FRAMER0(semQGetInst)(sp)         /* save v0 */
        sw      v1, WV_FRAMER1(semQGetInst)(sp)         /* save v1 */
        sw      t2, WV_FRAMER2(semQGetInst)(sp) 	/* save t2 */
        sw      t3, WV_FRAMER3(semQGetInst)(sp)         /* save t3 */
        sw      t4, WV_FRAMER4(semQGetInst)(sp)         /* save t4 */
        sw      t5, WV_FRAMER5(semQGetInst)(sp)         /* save t5 */
        sw      t6, WV_FRAMER6(semQGetInst)(sp)         /* save t6 */
        sw      t7, WV_FRAMER7(semQGetInst)(sp)         /* save t7 */
        sw      t8, WV_FRAMER8(semQGetInst)(sp)         /* save t8 */
        sw      t9, WV_FRAMER9(semQGetInst)(sp)         /* save t9 */
        
	lw	t1, _func_evtLogM1      /* call event log routine */
	beqz	t1, trgCheckSemQGet

	move	a1, a0
	li	a0, EVENT_OBJ_SEMGIVE
	jal	t1			 /* call event log routine */

        /* now restore all the stuff we have saved */

        lw      t9, WV_FRAMER9(semQGetInst)(sp)
        lw      t8, WV_FRAMER8(semQGetInst)(sp)
        lw      t7, WV_FRAMER7(semQGetInst)(sp)
        lw      t6, WV_FRAMER6(semQGetInst)(sp)
        lw      t5, WV_FRAMER5(semQGetInst)(sp)
        lw      t4, WV_FRAMER4(semQGetInst)(sp)
        lw      t3, WV_FRAMER3(semQGetInst)(sp)
        lw      t2, WV_FRAMER2(semQGetInst)(sp)
        lw      v1, WV_FRAMER1(semQGetInst)(sp)
        lw      v0, WV_FRAMER0(semQGetInst)(sp)

	lw	ra, WV_FRAMERA(semQGetInst)(sp)
	addu    sp, WV_FRAMESZ(semQGetInst)

trgCheckSemQGet:

        lw      t0, trgEvtClass
        li      t1, TRG_CLASS_2_ON
        and     t0, t0, t1
        bne     t1, t0, restoreSemQGetEvt

	li	a0, EVENT_OBJ_SEMGIVE
	li      a1, TRG_CLASS2_INDEX
	li      a2, 0x0
	LW      a3, FRAMEA0(semQGet)(sp)

        WV_SETFRAME(semQGetTrg,14)
        subu    sp, WV_FRAMESZ(semQGetTrg) /* create stack frame */
        sw      ra, WV_FRAMERA(semQGetTrg)(sp) /* save ra */

        sw      zero, WV_FRAMER0(semQGetTrg)(sp)         /* save v0 */
        sw      zero, WV_FRAMER1(semQGetTrg)(sp)         /* save v0 */
        sw      zero, WV_FRAMER2(semQGetTrg)(sp)         /* save v0 */
        sw      zero, WV_FRAMER3(semQGetTrg)(sp)         /* save v0 */

        /* now save the rest of the volatile register set. 
         * This is to ensure that when we come back from the C
         * function callout, all the register are as they were
         * before the call since we don't know what registers
         * the C functions have trashed that the assembly files
         * might rely on.
         */

       /* 
         * no need to save t0 (is saved above) & t1 (gets trashed anyway).
        */

        sw      v0, WV_FRAMER4(semQGetTrg)(sp)         /* save v0 */
        sw      v1, WV_FRAMER5(semQGetTrg)(sp)         /* save v1 */
        sw      t2, WV_FRAMER6(semQGetTrg)(sp) 	/* save t2 */
        sw      t3, WV_FRAMER7(semQGetTrg)(sp)         /* save t3 */
        sw      t4, WV_FRAMER8(semQGetTrg)(sp)         /* save t4 */
        sw      t5, WV_FRAMER9(semQGetTrg)(sp)         /* save t5 */
        sw      t6, WV_FRAMER10(semQGetTrg)(sp)         /* save t6 */
        sw      t7, WV_FRAMER11(semQGetTrg)(sp)         /* save t7 */
        sw      t8, WV_FRAMER12(semQGetTrg)(sp)         /* save t8 */
        sw      t9, WV_FRAMER13(semQGetTrg)(sp)         /* save t9 */

        lw      t1, _func_trgCheck
        jal     t1                              /* call trgCheck routine */

        /* now restore all the stuff we have saved */

        lw      t9, WV_FRAMER13(semQGetTrg)(sp)
        lw      t8, WV_FRAMER12(semQGetTrg)(sp)
        lw      t7, WV_FRAMER11(semQGetTrg)(sp)
        lw      t6, WV_FRAMER10(semQGetTrg)(sp)
        lw      t5, WV_FRAMER9(semQGetTrg)(sp)
        lw      t4, WV_FRAMER8(semQGetTrg)(sp)
        lw      t3, WV_FRAMER7(semQGetTrg)(sp)
        lw      t2, WV_FRAMER6(semQGetTrg)(sp)
        lw      v1, WV_FRAMER5(semQGetTrg)(sp)
        lw      v0, WV_FRAMER4(semQGetTrg)(sp)

	lw	ra, WV_FRAMERA(semQGetTrg)(sp)
	addu    sp, WV_FRAMESZ(semQGetTrg)
restoreSemQGetEvt:

	lw	t0, FRAMER0(semQGet)(sp)
	LW	a0, FRAMEA0(semQGet)(sp)
noSemQGetEvt:
	/* windview instrumentation - END */

#endif /* WV_INSTRUMENTATION */
	HAZARD_VR5400
	mtc0	t0, C0_SR		/* UNLOCK INTS */
	la	a0, SEM_Q_HEAD(a0)	/* pass pointer to qHead */
	jal	windPendQGet		/* unblock someone */
	jal	windExit		/* KERNEL EXIT */
	LW	ra, FRAMERA(semQGet)(sp) /* restore ra */
	addu	sp, FRAMESZ(semQGet)	/* clean up stack */
	j	ra			/* windExit sets v0 */
	.end	semQGet

/*******************************************************************************
*
* semQPut - block current task on the semaphore queue head
*/

	.ent	semQPut
semQPut:
	SETFRAME(semQPut,1)
	subu	sp, FRAMESZ(semQPut)	/* create stack frame */
	sw	a1, FRAMEA1(semQPut)(sp) /* save timeout */
	sw	a0, FRAMEA0(semQPut)(sp) /* save semId */
	SW	ra, FRAMERA(semQPut)(sp) /* save ra */
	li	t1, 1			/* load TRUE */
	sw	t1, kernelState		/* KERNEL ENTER */
#ifdef WV_INSTRUMENTATION
	/* windview instrumentation - BEGIN
	 * semTake level 2 (task transition state event )
	 */
        lw      t1, evtAction           /* is WV and-or triggering on? */
        beqz    t1, noSemQPutEvt

	sw	t0, FRAMER0(semQPut)(sp) /* save old sr value */

        /* we are checking
         * if ((wvEvtClass&(WV_CLASS_2_ON)) != (WV_CLASS_2_ON))
         * leave WV instrumentation and check triggering
         */

        lw      t0, wvEvtClass
        li      t1, WV_CLASS_2_ON
        and     t0, t0, t1
        bne     t1, t0, trgCheckSemQPut

        WV_SETFRAME(semQPutInst,10)
        subu    sp, WV_FRAMESZ(semQPutInst) /* create stack frame */
        sw      ra, WV_FRAMERA(semQPutInst)(sp) /* save ra */

        /* now save the rest of the volatile register set. 
         * This is to ensure that when we come back from the C
         * function callout, all the register are as they were
         * before the call since we don't know what registers
         * the C functions have trashed that the assembly files
         * might rely on.
         */

       /* 
         * no need to save t0 (is saved above) & t1 (gets trashed anyways).
         */

        sw      v0, WV_FRAMER0(semQPutInst)(sp)         /* save v0 */
        sw      v1, WV_FRAMER1(semQPutInst)(sp)         /* save v1 */
        sw      t2, WV_FRAMER2(semQPutInst)(sp)         /* save t2 */
        sw      t3, WV_FRAMER3(semQPutInst)(sp)         /* save t3 */
        sw      t4, WV_FRAMER4(semQPutInst)(sp)         /* save t4 */
        sw      t5, WV_FRAMER5(semQPutInst)(sp)         /* save t5 */
        sw      t6, WV_FRAMER6(semQPutInst)(sp)         /* save t6 */
        sw      t7, WV_FRAMER7(semQPutInst)(sp)         /* save t7 */
        sw      t8, WV_FRAMER8(semQPutInst)(sp)         /* save t8 */
        sw      t9, WV_FRAMER9(semQPutInst)(sp)         /* save t9 */
        
	lw	t1, _func_evtLogM1      /* call event log routine */
	beqz	t1, trgCheckSemQPut

	move	a1, a0
	li	a0, EVENT_OBJ_SEMTAKE
	jal	t1			/* call event log routine */

        /* now restore all the stuff we have saved */

        lw      t9, WV_FRAMER9(semQPutInst)(sp)
        lw      t8, WV_FRAMER8(semQPutInst)(sp)
        lw      t7, WV_FRAMER7(semQPutInst)(sp)
        lw      t6, WV_FRAMER6(semQPutInst)(sp)
        lw      t5, WV_FRAMER5(semQPutInst)(sp)
        lw      t4, WV_FRAMER4(semQPutInst)(sp)
        lw      t3, WV_FRAMER3(semQPutInst)(sp)
        lw      t2, WV_FRAMER2(semQPutInst)(sp)
        lw      v1, WV_FRAMER1(semQPutInst)(sp)
        lw      v0, WV_FRAMER0(semQPutInst)(sp)

	lw	ra, WV_FRAMERA(semQPutInst)(sp)
	addu    sp, WV_FRAMESZ(semQPutInst)

trgCheckSemQPut:

        lw      t0, trgEvtClass
        li      t1, TRG_CLASS_2_ON
        and     t0, t0, t1
        bne     t1, t0, restoreSemQPutEvt

        li      a0, EVENT_OBJ_SEMTAKE
	li      a1, TRG_CLASS2_INDEX
	li      a2, 0x0
	LW      a3, FRAMEA0(semQPut)(sp)

        WV_SETFRAME(semQPutTrg,10)
        subu    sp, WV_FRAMESZ(semQPutTrg) /* create stack frame */
        sw      ra, WV_FRAMERA(semQPutTrg)(sp) /* save ra */

        /* now save the rest of the volatile register set. 
         * This is to ensure that when we come back from the C
         * function callout, all the register are as they were
         * before the call since we don't know what registers
         * the C functions have trashed that the assembly files
         * might rely on.
         */

       /* 
         * no need to save t0 (is saved above) & t1 (gets trashed anyways).
         */

        sw      v0, WV_FRAMER0(semQPutTrg)(sp)         /* save v0 */
        sw      v1, WV_FRAMER1(semQPutTrg)(sp)         /* save v1 */
        sw      t2, WV_FRAMER2(semQPutTrg)(sp)         /* save t2 */
        sw      t3, WV_FRAMER3(semQPutTrg)(sp)         /* save t3 */
        sw      t4, WV_FRAMER4(semQPutTrg)(sp)         /* save t4 */
        sw      t5, WV_FRAMER5(semQPutTrg)(sp)         /* save t5 */
        sw      t6, WV_FRAMER6(semQPutTrg)(sp)         /* save t6 */
        sw      t7, WV_FRAMER7(semQPutTrg)(sp)         /* save t7 */
        sw      t8, WV_FRAMER8(semQPutTrg)(sp)         /* save t8 */
        sw      t9, WV_FRAMER9(semQPutTrg)(sp)         /* save t9 */

        lw      t1, _func_trgCheck
        jal     t1                              /* call trgCheck routine */

        /* now restore all the stuff we have saved */

        lw      t9, WV_FRAMER9(semQPutTrg)(sp)
        lw      t8, WV_FRAMER8(semQPutTrg)(sp)
        lw      t7, WV_FRAMER7(semQPutTrg)(sp)
        lw      t6, WV_FRAMER6(semQPutTrg)(sp)
        lw      t5, WV_FRAMER5(semQPutTrg)(sp)
        lw      t4, WV_FRAMER4(semQPutTrg)(sp)
        lw      t3, WV_FRAMER3(semQPutTrg)(sp)
        lw      t2, WV_FRAMER2(semQPutTrg)(sp)
        lw      v1, WV_FRAMER1(semQPutTrg)(sp)
        lw      v0, WV_FRAMER0(semQPutTrg)(sp)

	lw	ra, WV_FRAMERA(semQPutTrg)(sp)
	addu    sp, WV_FRAMESZ(semQPutTrg)

restoreSemQPutEvt:
	lw	t0, FRAMER0(semQPut)(sp)
	lw	a0, FRAMEA0(semQPut)(sp)
	lw	a1, FRAMEA1(semQPut)(sp)

noSemQPutEvt:
	/* windview instrumentation - END */

#endif /* WV_INSTRUMENTATION */
	.set	noreorder
	HAZARD_VR5400
	mtc0	t0, C0_SR		/* UNLOCK INTS */
	.set	reorder
	addiu	a0, SEM_Q_HEAD		/* pass pointer to qHead */
	jal	windPendQPut		/* block on semaphore */
	bne	zero, v0, semQPutFail	/* if (return != OK) */
	jal	windExit		/* else KERNEL EXIT */
	li	t1, 1			/* load RESTART */
        lw	t0, _func_sigTimeoutRecalc /* load sig timeout rtn */
	beq	v0, t1, semRestart	/* again if windExit == RESTART */
	LW	ra, FRAMERA(semQPut)(sp) /* restore ra */
	addu	sp, FRAMESZ(semQPut)	/* clean up stack */
	j	ra			/* windExit sets v0 */
semQPutFail:
	jal	windExit		/* KERNEL EXIT */
	LW	ra, FRAMERA(semQPut)(sp) /* restore ra */
	li	v0, -1			/* return ERROR */
	addu	sp, FRAMESZ(semQPut)	/* clean up stack */
	j	ra			/* return to sender */
semRestart:
        lw	a0, FRAMEA1(semQPut)(sp) /* semRestart needs timeout param */
        jal	t0			/* recalc the timeout */
        move	a1, v0			/* and store it */
	lw	a0, FRAMEA0(semQPut)(sp) /* restore semId */
	LW	ra, FRAMERA(semQPut)(sp)/* restore ra */
	addu	sp, FRAMESZ(semQPut)	/* clean up stack */
        j	semTake                 /* start the whole thing over */
	.end	semQPut

/*******************************************************************************
*
* semOTake - VxWorks 4.x semTake
*
* Optimized version of semOTake.  This inserts the necessary argument of
* WAIT_FOREVER for semBTake.
*/

	.ent	semOTake
semOTake:
	SETFRAME(semOTake,0)
	subu	sp, FRAMESZ(semOTake) /* create stack frame */
	SW	ra, FRAMERA(semOTake)(sp) /* save ra */
	li	a1, -1		/* pass WAIT_FOREVER */
	jal	semBTake	/* call semTake */
	LW	ra, FRAMERA(semOTake)(sp) /* restore ra */
	addu	sp, FRAMESZ(semOTake) /* clean up stack frame */
	j	ra		/* and return */
	.end	semOTake

/*******************************************************************************
*
* semClear - VxWorks 4.x semClear
*
* Optimized version of semClear.  This inserts the necessary argument of
* NO_WAIT for semBTake.
*/

	.ent	semClear
semClear:
	SETFRAME(semClear,0)
	subu	sp, FRAMESZ(semClear) /* create stack frame */
	SW	ra, FRAMERA(semClear)(sp) /* save ra */
	move	a1, zero	/* pass NO_WAIT */
	jal	semBTake	/* do semBTake */
	LW	ra, FRAMERA(semClear)(sp) /* restore ra */
	addu	sp, FRAMESZ(semClear) /* clean stack frame */
	j	ra		/* cleanup */
	.end	semClear

#endif /* semALib_PORTABLE */