semalib.s

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

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

	lbu	v1, SEM_TYPE(a0)		/* put the sem class into v1 */
	bne	zero, v1, semGiveNotBinary	/* opt for BINARY if v1==0 */

		/* BINARY SEMAPHORE OPTIMIZATION */

semBGive:					/* a0 = semId! */
	HAZARD_VR5400
	mfc0	t0, C0_SR
	li	t1, ~SR_INT_ENABLE
	and	t1, t1, t0
	mtc0	t1, C0_SR			/* LOCK INTERRUPTS */
	HAZARD_INTERRUPT
	la	t3, semClass			/* get semClass address */
	lw	t2, SEM_CLASS(a0)		/* read semIdClass */
	bne	t2, t3, semIsInvalidUnlock	/* valid semaphore? */
1:	lw	v0, SEM_Q_HEAD(a0)		/* */
	lw	t2, SEM_STATE (a0)		/* pOwner */
	sw	v0, SEM_STATE(a0)		/* */
	bne	zero, v0, semQGet		/* if not empty get from Q */
 
	lw	t3, SEM_EVENTS_TASKID (a0)	/* Branch if either previous */
	beq	zero, t2, semGiveReturn		/* semOwner or taskId are */
	beq	zero, t3, semGiveReturn		/* NULL. */

/*
 * a0 - semId
 * t0 - was used with interrupts
 * t1 - was used with interrupts
 * t3 - semId->events.taskId
 * v0 - NULL
 * v1 - NULL
 */
semEvRsrcSend:
	li      t4, 1
	sw      t4, kernelState                 /* KERNEL ENTER */

	HAZARD_VR5400
	mtc0  t0, C0_SR                         /* UNLOCK INTERRUPTS */
	HAZARD_INTERRUPT

	lw      t4, errno
	SETFRAME(semBGive,3)                    /* Create stack frame      */
	subu    sp, FRAMESZ(semBGive)           /* Create some stack space */
	SW      ra, FRAMERA(semBGive)(sp)       /* Save return address     */
	SW      t4, FRAMER1(semBGive)(sp)       /* Save errno              */
	SW      a0, FRAMER0(semBGive)(sp)       /* Save semId              */

	lw      a1, SEM_EVENTS_REGISTERED (a0)  /* semId->events.registered */
	move    a0, t3                          /* semId->events.taskId     */
	jal     eventRsrcSend                   /* eventRsrcSend (a0, a1)   */
 
	LW      a0, FRAMER0(semBGive)(sp)       /* Restore semId  */
	LW      t4, FRAMER1(semBGive)(sp)       /* Restore errno  */
 
	/*
	 * Recall that for boolean expressions, 0 & x = 0, and 1 & x = x.  The
	 * return value of eventRsrcSend () is either OK, or ERROR (0, -1).
	 * If evSendStatus is the return value ...
	 *              evSendStatus & x = {0, evSendStatus}.
	 * Thus after AND-ing, a single test can be performed.
	 *
	 * Also recall for the boolean expressions 0 | x = x, and 1 | x = 1.
	 *              evSendStatus | x = {x, 1}
	 */
 
	move    t3, zero                        /* Default return OK */
 
	lb      t5, SEM_OPTIONS (a0)            /* Combine two tests into */
	and     t5, t5, v0                      /* one.  Refer to previous */
	andi    t5, SEM_EVENTSEND_ERR_NOTIFY    /* comments above. */
	beq     t5, zero, semBGiveEvtOptions
 
	li	t4, S_eventLib_EVENTSEND_FAILED       /* Set new errno */
 
	sub     t3, zero, 1                     /* New return value */
	sw      zero, SEM_EVENTS_TASKID (a0)    /* Clear taskId */
	j       semBGiveEvtOptionsDone
 
semBGiveEvtOptions:
	lb      t5, SEM_EVENTS_OPTIONS (a0)     /* Combine two tests into */
	or      t5, t5, v0                      /* one.  Refer to previous */
	andi    t5, t5, EVENTS_SEND_ONCE        /* comments above. */
	beq     t5, zero, semBGiveEvtOptionsDone
	sw      zero, SEM_EVENTS_TASKID (a0)    /* Clear taskId */
 
semBGiveEvtOptionsDone:
	SW      t3, FRAMER2(semBGive)(sp)       /* Save retVal to stack */
	SW      t4, FRAMER1(semBGive)(sp)       /* Save errno to stack */
 
	jal     windExit                        /* KERNEL EXIT */
 
	LW      ra, FRAMERA(semBGive)(sp)       /* Restore return address */
	LW      a0, FRAMER0(semBGive)(sp)       /* Restore semId */
	LW      t4, FRAMER1(semBGive)(sp)       /* Restore errno */
	LW      v0, FRAMER2(semBGive)(sp)       /* Restore return value */
	addu    sp, FRAMESZ(semBGive)           /* Remove stack frame */
	sw      t4, errno                       /* Update errno */
 
	j       ra                              /* Return */

	/* NEVER FALL THROUGH */

semGiveReturn:
        HAZARD_VR5400
        mtc0    t0, C0_SR               /* UNLOCK INTS */
        j       ra                      /* return, v0 still 0 for OK */

	/* NEVER FALL THROUGH */

semGiveNotBinary:

        /* call semGive indirectly via semGiveTbl.  Note that the index could
	 * equal zero after it is masked.  semBGive is the zeroeth element
	 * of the table, but for it to function correctly in the optimized
	 * version above, we must be certain not to clobber a0.  Note, also
	 * that old semaphores will also call semBGive above.
	 */

	andi	v1, 0x7			/* mask v1 */
	la	t1, semGiveTbl		/* get table address into t1 */
	sll	v1, 2			/* make word indexed */
	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	semGive

/*******************************************************************************
*
* semIsInvalid - unlock interupts and call semInvalid ().
*/

semIsInvalidUnlock:
	HAZARD_VR5400
	mtc0	t0, C0_SR		/* UNLOCK INTS */
semIsInvalid:
	j	semInvalid		/* let C rtn do work and rts */

/*******************************************************************************
*
* semNULLRestrict - set errno and return ERROR
*
* The C routine for semInvalid does not work in the case where a semID is
* NULL. It will attempt to ref the NULL semID. Instead, set errno manually & 
* return
*/
semNULLRestrict:
	li	t0, S_objLib_OBJ_ID_ERROR	/* load error ID */
	sw	t0, errno			/* set errno */
	li	v0, ERROR			/* return ERROR */
	j	ra

/*******************************************************************************
*
* semTakeIntRestrict - call semIntRestrict ().
*/
semTakeIntRestrict:
	j	semIntRestrict		/* let C rtn do work and rts */


/*******************************************************************************
*
* semTake - take a semaphore
*

*STATUS semTake (semId, timeout)
*    SEM_ID semId;		/* semaphore id to give *
*    ULONG  timeout;		/* timeout in ticks *

*/

	.ent	semTake
semTake:
	beq	a0, zero, semNULLRestrict	/* dont allow NULL ref */
	lw	t0, intCnt			/* read intCnt */
	bne	zero, t0, semTakeIntRestrict	/* intCnt > 0, no isr use */
	andi	t0, a0, 1			/* check if smObj id */
	beq	t0, zero, 2f			/* t0==zero, local semaphore */
	lw	t1, smObjPoolMinusOne		/* load local pool address */
	SETFRAME(semTake,0)
	subu	sp, FRAMESZ(semTake)		/* carve stack space */
	addu	a0, a0, t1			/* convert id to local addr */
	SW	ra, FRAMERA(semTake)(sp)	/* save return address */
	/* shared semaphore types are stored in network order
	 * we are only interested in the bottom byte so the
	 * following works in both bigendian and littleendian configurations
	 */
	lbu	t2, SEM_SM_TYPE+3(a0)		/* get semaphore type in t2 */
	andi	t2, 0x7				/* mask t2 */
	la	t3, semTakeTbl			/* load semaphore give table */
	sll	t2, 2				/* scale by size of (FUNCPTR) */
	addu	t3, t2, t3			/* appropriate give table */
	lw	t4, (t3)			/* get appropriate take table */
	jal	t4				/* call take func */
	LW	ra, FRAMERA(semTake)(sp)	/* get return address */
	addu 	sp, FRAMESZ(semTake)		/* pop up stack */
	j	ra
2:
#ifdef WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * semTake level 1 (object status event )
	 */
	lw	t0, evtAction		    /* is level 1 event collection on? */
	beqz	t0, noSemTakeEvt

        lw      t3, SEM_CLASS(a0)
        la      t0, semClass                    /* check validity */
        beq     t3, t0, objOkTake
        la      t0, semInstClass                /* check validity */
        bne     t3, t0, noSemTakeEvt            /* invalid semaphore */
objOkTake:

        /* we are checking
         * if ((wvEvtClass&(WV_CLASS_3_ON)) != (WV_CLASS_3_ON))
         * leave WV instrumentation and check triggering
         */
        lw      t0, wvEvtClass
        li      t4, WV_CLASS_3_ON
        and     t0, t0, t4
        bne     t4, t0, trgCheckSemTake

	/* is this semaphore object instrumented? */
        lw      t0, SEM_INST_RTN(t3)            /* event routine attached? */
	beqz	t0, noSemTakeEvt


	/* log event for this object */

        lhu     t2, SEM_RECURSE(a0)

        WV_SETFRAME(semTakeInst,11)
        subu    sp, WV_FRAMESZ(semTakeInst) /* create stack frame */
        sw      ra, WV_FRAMERA(semTakeInst)(sp) /* save ra */
        sw      a0, WV_FRAMEA0(semTakeInst)(sp)    /* and a0 */
        sw      a1, WV_FRAMEA1(semTakeInst)(sp)    /* and a0 */
        sw      t2, WV_FRAMER0(semTakeInst)(sp) /* pass t2, 5th arg to func */
        sw      zero, WV_FRAMER1(semTakeInst)(sp) /* pass 0 to 6th arg */
        sw      zero, WV_FRAMER2(semTakeInst)(sp) /* pass 0 to 7th arg */


        /* 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 & t4, gets trashed right after the
         * function call anyways.
         */

        sw      v0, WV_FRAMER3(semTakeInst)(sp)         /* save v0 */
        sw      t1, WV_FRAMER4(semTakeInst)(sp)         /* save t1 */
        sw      t3, WV_FRAMER5(semTakeInst)(sp)         /* save t3 */
        sw      t5, WV_FRAMER6(semTakeInst)(sp)         /* save t5 */
        sw      t6, WV_FRAMER7(semTakeInst)(sp)         /* save t6 */
        sw      t7, WV_FRAMER8(semTakeInst)(sp)         /* save t7 */
        sw      t8, WV_FRAMER9(semTakeInst)(sp)        /* save t8 */
        sw      t9, WV_FRAMER10(semTakeInst)(sp)        /* save t9 */

        lw      a3, SEM_STATE(a0)
        move    a2, a0                          /* a2 = semID */
        li      a1, 3                           /* # of args passed to func */
        li      a0, EVENT_SEMTAKE               /* event ID */

        jal     t0                              /* call routine */

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

        lw      t9, WV_FRAMER10(semTakeInst)(sp)
        lw      t8, WV_FRAMER9(semTakeInst)(sp)
        lw      t7, WV_FRAMER8(semTakeInst)(sp)
        lw      t6, WV_FRAMER7(semTakeInst)(sp)
        lw      t5, WV_FRAMER6(semTakeInst)(sp)
        lw      t3, WV_FRAMER5(semTakeInst)(sp)
        lw      t1, WV_FRAMER4(semTakeInst)(sp)
        lw      v0, WV_FRAMER3(semTakeInst)(sp)

        lw      t2, WV_FRAMER0(semTakeInst)(sp)
        lw      a0, WV_FRAMEA0(semTakeInst)(sp)
        lw      a1, WV_FRAMEA1(semTakeInst)(sp)
        lw      ra, WV_FRAMERA(semTakeInst)(sp)
        addu    sp, WV_FRAMESZ(semTakeInst)

trgCheckSemTake:

        lw      t0, trgEvtClass
        li      t4, TRG_CLASS_3_ON
        and     t0, t0, t4
        bne     t4, t0, noSemTakeEvt

        lhu     t2, SEM_RECURSE(a0)
        lw      t1, SEM_STATE(a0)

        WV_SETFRAME(semTakeTrg,11)
        subu    sp, WV_FRAMESZ(semTakeTrg) /* create stack frame */
        sw      ra, WV_FRAMERA(semTakeTrg)(sp) /* save ra */
        sw      a0, WV_FRAMEA0(semTakeTrg)(sp)    /* and a0 */
        sw      a1, WV_FRAMEA1(semTakeTrg)(sp)    /* and a0 */
        sw      t1, WV_FRAMER0(semTakeTrg)(sp) /* save t1, 5th arg to func */
        sw      t2, WV_FRAMER1(semTakeTrg)(sp) /* save t2, 6th arg to func */

        /* 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 & t4, since they are trashed above */

        sw      zero, WV_FRAMER2(semTakeTrg)(sp)        /* 7th arg to func */
        sw      zero, WV_FRAMER3(semTakeTrg)(sp)        /* 8th arg to func */

        sw      v0, WV_FRAMER4(semTakeTrg)(sp)         /* save v0 */
        sw      t3, WV_FRAMER5(semTakeTrg)(sp)         /* save t3 */
        sw      t5, WV_FRAMER6(semTakeTrg)(sp)         /* save t5 */
        sw      t6, WV_FRAMER7(semTakeTrg)(sp)         /* save t6 */
        sw      t7, WV_FRAMER8(semTakeTrg)(sp)        /* save t7 */
        sw      t8, WV_FRAMER9(semTakeTrg)(sp)        /* save t8 */
        sw      t9, WV_FRAMER10(semTakeTrg)(sp)        /* save t9 */

        lw      a3, SEM_STATE(a0)
        move    a3, a0
        move    a2, a0

        li      a0, EVENT_SEMTAKE
	li      a1, TRG_CLASS3_INDEX

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

        /* Now restore */

        lw      t9, WV_FRAMER10(semTakeTrg)(sp)
        lw      t8, WV_FRAMER9(semTakeTrg)(sp)
        lw      t7, WV_FRAMER8(semTakeTrg)(sp)
        lw      t6, WV_FRAMER7(semTakeTrg)(sp)
        lw      t5, WV_FRAMER6(semTakeTrg)(sp)
        lw      t3, WV_FRAMER5(semTakeTrg)(sp)
        lw      v0, WV_FRAMER4(semTakeTrg)(sp)

        lw      t2, WV_FRAMER1(semTakeTrg)(sp)
        lw      t1, WV_FRAMER0(semTakeTrg)(sp)
        lw      a0, WV_FRAMEA0(semTakeTrg)(sp)
        lw      a1, WV_FRAMEA1(semTakeTrg)(sp)
        lw      ra, WV_FRAMERA(semTakeTrg)(sp)
        addu    sp, WV_FRAMESZ(semTakeTrg)

noSemTakeEvt:
	/* windview instrumentation - END */

#endif /* WV_INSTRUMENTATION */
	lbu	v1, SEM_TYPE(a0)		/* get semaphore class
						   into v1 */
	bne	zero, v1, semTakeNotBinary	/* optimize binary
						   semaphore v1 == 0 */
		/* BINARY SEMAPHORE OPTIMIZATION */
semBTake:					/* a0 = semId! */
	.set	noreorder
	HAZARD_VR5400
	mfc0	t0, C0_SR
	HAZARD_CP_READ
	li	t1, ~SR_INT_ENABLE
	and	t1, t1, t0
	mtc0	t1, C0_SR			/* LOCK INTERRUPTS */
	HAZARD_INTERRUPT
	la	t3, semClass			/* get semClass address */
	.set	reorder
	lw	t2, SEM_CLASS(a0)		/* read semIdClass */
	bne	t2, t3, semIsInvalidUnlock	/* valid semaphore? */
1:	lw	v0, SEM_STATE(a0)		/* test for owner */
	bne	zero, v0, semQPut		/* if sem is owned we block */
	lw	t3, taskIdCurrent		/* get tidc */
	sw	t3, SEM_STATE(a0)		/* now we own it */
	HAZARD_VR5400
	mtc0	t0, C0_SR			/* UNLOCK INTS */
	HAZARD_INTERRUPT
	j	ra				/* return, v0 still zero */
semTakeNotBinary:
	andi	v1, 0x7			/* mask v1 to sane value */
	la	t1, semTakeTbl		/* get table address into t1 */
	sll	v1, 2			/* make word indexed */