semalib.s

vxworks source code, used for develop vxworks system.

	BNE	resumeSemTake		/* branch if not */

	STMFD	sp!, {r0,r1,lr}		/* save semId, timeout, link */

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

	MOV	r2, #0
	MOV	r3, #0
	STMFD	sp!, {r2,r3}		/* push 2 dummy args */
	STMFD	sp!, {r2,r3}		/* push 2 dummy args .. again */
					/* r3 = NULL from above */
	MOV	r2, r0			/* r2 = semId */
	MOV	r1, #TRG_CLASS3_INDEX	/* r1 = Class */
	LDR	r0, L$_EVENT_SEMTAKE	/* r0 = event type */

	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 parameters from stack */
	LDMFD	sp!, {r0,r1,lr}		/* restore semId, timeout and link */

	B	resumeSemTake

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

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

FUNC_LABEL(semQGet)
	/*
	 * r0-> semaphore
	 * r3 = previous cpsr
	 * lr = return address
	 * interrupts are locked
	 */

	MOV	r2, #1			/* KERNEL ENTER */
	LDR	r12, L$_kernelState
	STR	r2, [r12]

#ifdef WV_INSTRUMENTATION

        /* windview instrumentation - BEGIN
         * semGive level 2 (task transition state event)
         */

	LDR	r2, L$_evtAction	/* is event logging and or triggering?*/
	LDR	r2, [r2]
	TEQS	r2, #0
	BNE	instrumentSemQGet	/* branch if so */

	/* instrumentation currently disabled */

resumeSemQGet:

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


	MSR	cpsr, r3		/* UNLOCK INTERRUPTS */
	STMFD	sp!, {lr}		/* save link */
	ADD	r0, r0, #SEM_Q_HEAD	/* r0-> qHead */
#if	(ARM_THUMB)
	LDR	r12, L$_windPendQGet	/* unblock someone */
	BL	FUNC(arm_call_via_r12)	/* returns in ARM state */
#else
	BL	FUNC(windPendQGet)		/* unblock someone */
#endif	/* ARM_THUMB */

	LDMFD	sp!, {lr}		/* restore link */
	B	FUNC(windExit)		/* KERNEL EXIT */

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

#ifdef WV_INSTRUMENTATION

        /* windview instrumentation - BEGIN
         * semGive level 2 (task transition state event)
         */

instrumentSemQGet:

        LDR	r2, L$_wvEvtClass	/* is instrumentation on? */
	LDR	r2, [r2]
        AND	r2, r2, #WV_CLASS_2_ON	
        TEQS	r2, #WV_CLASS_2_ON	
	BNE	trgSemQGetEvt		/* branch if not */

	LDR	r12, L$__func_evtLogM1	/* event log routine exists ? */
	LDR	r12, [r12]
	TEQS	r12, #0
	BEQ	trgSemQGetEvt		/* branch if nothing to call */

	STMFD	sp!, {r0,r3,lr}		/* save regs */
	MOV	r1, r0			/* r1 = semId */
	MOV	r0, #EVENT_OBJ_SEMGIVE	/* r0 = event id */

#if	(ARM_THUMB)
	BL	FUNC(arm_call_via_r12)	/* call function */
#else
	MOV	lr, pc			/* call function */
	MOV	pc, r12
#endif	/* ARM_THUMB */

	LDMFD	sp!, {r0,r3,lr}		/* restore regs */

trgSemQGetEvt: 
        LDR	r2, L$_trgEvtClass	/* is triggering on? */
	LDR	r2, [r2]
	LDR	r1, L$_TRG_CLASS_2_ON
        AND	r2, r2, r1
        TEQS	r2, r1
	BNE	resumeSemQGet		/* branch if not */

	STMFD	sp!, {r0,r3,lr}		/* save regs */

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

	MOV	r2, #0
	MOV	r3, #0
	STMFD	sp!, {r2,r3}		/* push 2 dummy args */
	STMFD	sp!, {r2,r3}		/* push 2 dummy args .. again */
						/* r3 = NULL from above */
	MOV	r2, r0				/* r2 = semId */
	MOV	r1, #TRG_CLASS2_INDEX		/* r1 = Class */
	MOV	r0, #EVENT_OBJ_SEMGIVE		/* r0 = event type */

	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 parameters from stack */
	LDMFD	sp!, {r0,r3,lr}		/* restore regs */
	B	resumeSemQGet

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

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

FUNC_LABEL(semQPut)
	/*
	 * r0-> semaphore
	 * r1 = timeout
	 * r3 = previous cpsr
	 * interrupts are locked
	 */

	MOV	r2, #1			/* KERNEL ENTER */
	LDR	r12, L$_kernelState
	STR	r2, [r12]

#ifdef WV_INSTRUMENTATION

        /* windview instrumentation - BEGIN
         * semTake level 2 (task transition state event)
         */
	
	LDR	r2, L$_evtAction	/* is event logging and or triggering?*/
	LDR	r2, [r2]
	TEQS	r2, #0
	BNE	instrumentSemQPut	/* branch if so */

	/* instrumentation currently disabled */

resumeSemQPut:

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


	MSR	cpsr, r3		/* UNLOCK INTERRUPTS */
	STMFD	sp!, {r0,r1,lr}
	ADD	r0, r0, #SEM_Q_HEAD	/* r0-> qHead */
#if	(ARM_THUMB)
	LDR	r12, L$_windPendQGet	/* unblock someone */
	BL	FUNC(arm_call_via_r12)	/* returns in ARM state */
#else
	BL	FUNC(windPendQPut)		/* block on the semaphore */
#endif	/* ARM_THUMB */

	TEQS	r0, #0			/* succeeded? */
	BNE	semQPutFail		/* branch if failed */

	/* succeeded */

	BL	FUNC(windExit)		/* KERNEL EXIT */
	TEQS	r0, #RESTART		/* if not RESTART.. */
	ADDNE	sp, sp, #8		/* ..strip stack.. */
	LDMNEFD	sp!, {pc}		/* ..and return */

	/* restart: recalculate the timeout and try again */

	LDR	r0, [sp, #4]		/* get timeout from stack */
	LDR	r1, L$__func_sigTimeoutRecalc

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

	MOV	r1, r0			/* r1 = timeout */
	LDMFD	sp!, {r0,r2,lr}		/* restore r0,lr/flatten stack */
	B	FUNC(semTake)		/* go again */

	/* NEVER FALL THROUGH */

semQPutFail:
	BL	FUNC(windExit)		/* KERNEL EXIT */
	MOV	r0, #ERROR		/* return ERROR */
	ADD	sp, sp, #8		/* strip stack */
	LDMFD	sp!, {pc}		/* return */

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

#ifdef WV_INSTRUMENTATION

        /* windview instrumentation - BEGIN
         * semTake level 2 (task transition state event)
         */

instrumentSemQPut:

        LDR	r2, L$_wvEvtClass	/* is instrumentation on? */
	LDR	r2, [r2]
        AND	r2, r2, #WV_CLASS_2_ON	
        TEQS	r2, #WV_CLASS_2_ON	
	BNE	trgSemQPutEvt		/* branch if not */

        LDR	r12, L$__func_evtLogM1	/* call event log routine */
	LDR	r12, [r12]
	TEQS	r12, #0
	BEQ	trgSemQPutEvt		/* branch if nothing to call */

	STMFD	sp!, {r0,r1,r3,lr}	/* save regs */
	MOV	r1, r0			/* r1 = semId */
	LDR	r0, L$_EVENT_OBJ_SEMTAKE /* r0 = event id */

#if	(ARM_THUMB)
	BL	FUNC(arm_call_via_r12)	/* call function */
#else
	MOV	lr, pc			/* call function */
	MOV	pc, r12
#endif	/* ARM_THUMB */

	LDMFD	sp!, {r0,r1,r3,lr}	/* restore regs */

trgSemQPutEvt: 
        LDR	r2, L$_trgEvtClass	/* is triggering on? */
	LDR	r2, [r2]
	LDR	r12, L$_TRG_CLASS_2_ON
        AND	r2, r2, r12
        TEQS	r2, r12
	BNE	resumeSemQPut		/* branch if not */

	STMFD	sp!, {r0,r1,r3,lr}	/* save regs */

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

	MOV	r2, #0
	MOV	r3, #0
	STMFD	sp!, {r2,r3}		/* push 2 dummy args */
	STMFD	sp!, {r2,r3}		/* push 2 dummy args .. again */
						/* r3 = NULL from above */
	MOV	r2, r0				/* r2 = semId */
	MOV	r1, #TRG_CLASS2_INDEX		/* r1 = Class */
	LDR	r0, L$_EVENT_OBJ_SEMTAKE 	/* r0 = event type */

	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 parameters from stack */
	LDMFD	sp!, {r0,r1,r3,lr}	/* restore regs */

	B	resumeSemQPut

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

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

semIsInvalidUnlock:
	/*
	 * r0-> semaphore
	 * r3 = previous cpsr
	 * lr = return address
	 * interrupts are locked
	 */

	MSR	cpsr, r3		/* UNLOCK INTERRUPTS */
	B	FUNC(semInvalid)		/* let C function do work */

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

* STATUS semOTake
*	(
*	SEM_ID semId
*	)

*/

FUNC_LABEL(semOTake)
	MOV	r1, #WAIT_FOREVER
	B	FUNC(semBTake)

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

* STATUS semClear
*	(
*	SEM_ID semId
*	)

*/

FUNC_LABEL(semClear)
	MOV	r1, #NO_WAIT
	B	FUNC(semBTake)

#endif /* ! semALib_PORTABLE */