semalib.s

vxworks source code, used for develop vxworks system.

#ifdef WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * semGive level 1 (object status event)
	 */

instrumentSemGive:

	/* is this a valid semaphore? */

	LDR	r2, [r0]		/* get class of semaphore */
	LDR	r3, L$_semClass		/* get address of semClass */
	TEQS	r3, r2			/* if not, semClass.. */
	LDRNE	r3, L$_semInstClass	/* ..get address of semInstClass */
	TEQNES	r3, r2
	BNE	resumeSemGive		/* branch if invalid */

	/* This is a valid semaphore and some sort of instrumentation is on */
	
	STMFD	sp!, {r0,lr}		/* save semId and link */

        /* Check if we are at a high enough logging level */

        LDR	lr, L$_wvEvtClass	/* is instrumentation on? */
	LDR	lr, [lr]
        AND	lr, lr, #WV_CLASS_3_ON	
        TEQS	lr, #WV_CLASS_3_ON	
	BNE	trgSemGiveEvt		/* branch if not */

	/* is this semaphore object instrumented? */

	LDR	r12, [r2, #SEM_INST_RTN] /* event routine attached? */
	TEQS	r12, #0
	BEQ	trgSemGiveEvt		/* branch if no event routine */

	/* 
	 * log event for this object
	 * There are 7 arguments to this function
	 * 4 are passed in registers r0 to r3
	 * 3 are passed on the stack
	 */

	STMFD	sp!, {r0}		/* save semId to stack */

	MOV	r3, #0			/* push 2 dummy args */
	MOV	r2, #0

#if ARM_HAS_HALFWORD_INSTRUCTIONS
	LDRH	r1, [r0, #SEM_RECURSE]	/* r1 = recursion count */
#else
	LDR	r1, [r0, #(SEM_RECURSE & ~3)] /* word-aligned load */
#if (_BYTE_ORDER == _BIG_ENDIAN)
#if ((SEM_RECURSE & 2) != 0)
	MOV	r1, r1, LSL #16		/* so next instr will clear b31..b16 */
#endif
	MOV	r1, r1, LSR #16		/* r1[15..0] = recursion count */
#else	/* _BYTE_ORDER */
#if ((SEM_RECURSE & 2) == 0)
	MOV	r1, r1, LSL #16		/* so next instr will clear b31..b16 */
#endif
	MOV	r1, r1, LSR #16		/* r1[15..0] = recursion count */
#endif	/* _BYTE_ORDER */
#endif	/* ARM_HAS_HALFWORD_INSTRUCTIONS */

	/*
	 * Push the 3 stack-based parameters in r1-r3
	 * They are pushed in order r3, r2, r1
	 */

	STMFD	sp!, {r1-r3}

	LDR	r3, [r0, #SEM_STATE]	/* r3 = semaphore state */
	MOV	r2, r0			/* r2 = semId */
	MOV	r1, #3			/* nParam */
	LDR	r0, L$_EVENT_SEMGIVE	/* r0 = eventId */

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

	ADD	sp, sp, #12		/* strip 3 parameters from stack */
	LDMFD	sp!, {r0}		/* restore semId from stack*/

trgSemGiveEvt:
        LDR	r2, L$_trgEvtClass	/* is triggering on? */
	LDR	r2, [r2]
	LDR	r3, L$_TRG_CLASS_3_ON
        AND	r2, r2, r3	
        TEQS	r2, r3
	BNE	semGiveTidy		/* branch if not */

	/*
	 * 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_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 */
semGiveTidy:
	LDMFD	sp!, {r0,lr}		/* restore semId and link */

	B	resumeSemGive

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

/*******************************************************************************
*
* semTake - take a semaphore
*
* This routine performs the take operation on a specified semaphore.
* Depending on the type of semaphore, the state of the semaphore and
* the calling task may be affected.  The behavior of semTake() is
* discussed fully in the library description of the specific semaphore
* type being used.
*
* A timeout in ticks may be specified.  If a task times out, semTake() will
* return ERROR.  Timeouts of WAIT_FOREVER (-1) and NO_WAIT (0) indicate to wait
* indefinitely or not to wait at all.
*
* When semTake() returns due to timeout, it sets the errno to
* S_objLib_OBJ_TIMEOUT (defined in objLib.h).
*
* The semTake() routine is not callable from interrupt service routines.
* In this case it sets errno to S_intLib_NOT_ISR_CALLABLE and returns ERROR
*
* RETURNS: OK, or ERROR if the semaphore ID is invalid or the task timed out.

* STATUS semTake
*	(
*	SEM_ID	semId,		/@ semaphore ID to take @/
*	ULONG	timeout		/@ timeout in ticks @/
*	)

*/

FUNC_LABEL(semTake)

	TSTS	r0, #1			/* shared semaphore? */
	BNE	semTakeGlobal		/* branch if shared */

	/* Semaphore is local */

#ifdef WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * semTake level 1 (object status event)
	 */

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

resumeSemTake:

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


	LDRB	r12, [r0, #SEM_TYPE]	/* get semType */
	ANDS	r12, r12, #SEM_TYPE_MASK
	BNE	semTakeNotBinary	/* branch if not binary */

	/*
	 * BINARY SEMAPHORE OPTIMISATION
	 * FALL THROUGH	to semBTake
	 */

/*******************************************************************************
*
* semBTake - take a binary semaphore
*
* Takes the semaphore.  If the semaphore is empty, i.e., it has not been given
* since the last semTake() or semInit(), this task will become pended until
* the semaphore becomes available by some other task doing a semGive()
* of it.  If the semaphore is already available, this call will empty
* the semaphore, so that no other task can take it until this task gives
* it back, and this task will continue running.
*
* WARNING
* This routine may not be used from interrupt level.
*
* RETURNS: OK, or ERROR if the semaphore ID is invalid.

* STATUS semBTake
*	(
*	SEM_ID	semId,		/@ semaphore ID to give @/
*	int	timeout		/@ timeout in ticks @/
*	)

*/

FUNC_LABEL(semBTake)

	/* check if called from ISR */

	LDR	r2, L$_intCnt		/* restrict ISR use */
	LDR	r2, [r2]
	TEQS	r2, #0
	BNE	FUNC(semIntRestrict)	/* if ISR, let C function do the work */

	/* LOCK INTERRUPTS */

	MRS	r3, cpsr
	ORR	r12, r3, #I_BIT
	MSR	cpsr, r12

	LDR	r2, [r0]		/* get class of semaphore */
	LDR	r12, L$_semClass	/* get address of semClass */
	TEQS	r12, r2
#ifdef WV_INSTRUMENTATION
	LDRNE	r12, L$_semInstClass	/* if not semClass, check if.. */
	TEQNES	r12, r2			/* ..semInstClass */
#endif	/* WV_INSTRUMENTATION */
	BNE	semIsInvalidUnlock	/* branch if invalid */

	/* semaphore is valid */

	LDR	r12, [r0, #SEM_STATE]	/* test for owner */
	TEQS	r12, #0
	BNE	FUNC(semQPut)		/* if sem is owned, we block */

	LDR	r12, L$_taskIdCurrent	/* make this task the owner */
	LDR	r12, [r12]
	STR	r12, [r0, #SEM_STATE]	

	MSR	cpsr, r3		/* UNLOCK INTERRUPTS */
	MOV	r0, #OK			/* return OK */
	MOV	pc, lr

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

semTakeGlobal:
	/*
	 * semaphore is shared
	 * r0-> semaphore
	 * r1 = timeout
	 */

	LDR	r2, L$_smObjPoolMinusOne /* get address of smObjPoolMinusOne */
	LDR	r2, [r2]
	ADD	r0, r0, r2		/* convert id to local address */

	/*
	 * get semaphore type from SM_SEM_ID (struct sm_semaphore): this
	 * is a 32-bit word in network byte order (unlike in SEMAPHORE where
	 * it's a single byte)
	 */
#if (_BYTE_ORDER == _BIG_ENDIAN)
	LDR 	r12, [r0, #SEM_TYPE]	/* get sem type in network byte order */
#else
	LDRB	r12, [r0, #SEM_TYPE+3]	/* get sem type in network byte order */
#endif	/* _BYTE_ORDER */
	AND	r12, r12, #SEM_TYPE_MASK /* index into table */


semTakeNotBinary:
	/*
	 * r0-> semaphore (parameter for semaphore function)
	 * r1 = timeout
	 * r12 = index into semTakeTbl
	 * lr = return address
	 */

	LDR	r2, L$_semTakeTbl	/* get address of jump table */
	LDR	pc, [r2, r12, LSL #2]	/* jump to fn (semId in r0) */

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

#ifdef WV_INSTRUMENTATION

	/* windview instrumentation - BEGIN
	 * semTake level 1 (object status event)
	 */

instrumentSemTake:

	/* is this a valid semaphore? */

	LDR	r2, [r0]		/* get class of semaphore */
	LDR	r3, L$_semClass		/* get address of semClass */
	TEQS	r3, r2			/* if not, semClass.. */
	LDRNE	r3, L$_semInstClass	/* ..get address of semInstClass */
	TEQNES	r3, r2
	BNE	resumeSemTake		/* branch if invalid */

	/* This is a valid samaphore and some sort of instrumentation is on */

        /* Check if we are at a high enough logging level */

	LDR	r3, L$_wvEvtClass	/* is instrumentation on? */
	LDR	r3, [r3]
	AND	r3, r3, #WV_CLASS_3_ON	
	TEQS	r3, #WV_CLASS_3_ON	
	BNE	trgSemTakeEvt		/* branch if not */

	/* is this semaphore object instrumented? */

	LDR	r12, [r2, #SEM_INST_RTN] /* event routine attached? */
	TEQS	r12, #0
	BEQ	trgSemTakeEvt		/* branch if no event routine */

	/* 
	 * log event for this object
	 * There are 7 arguments to this function
	 * 4 are passed in registers r0 to r3
	 * 3 are passed on the stack
	 */

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

	MOV	r3, #0			/* push two dummy args */
	MOV	r2, #0			/* param 4, param5 */

#if ARM_HAS_HALFWORD_INSTRUCTIONS
	LDRH	r1, [r0, #SEM_RECURSE]	/* r1 = recursion count */
#else
	LDR	r1, [r0, #(SEM_RECURSE & ~3)] /* word-aligned load */
#if (_BYTE_ORDER == _BIG_ENDIAN)
#if ((SEM_RECURSE & 2) != 0)
	MOV	r1, r1, LSL #16		/* so next instr will clear b31..b16 */
#endif
	MOV	r1, r1, LSR #16		/* r1[15..0] = recursion count */
#else	/* _BYTE_ORDER */
#if ((SEM_RECURSE & 2) == 0)
	MOV	r1, r1, LSL #16		/* so next instr will clear b31..b16 */
#endif
	MOV	r1, r1, LSR #16		/* r1[15..0] = recursion count */
#endif	/* _BYTE_ORDER */
#endif	/* ARM_HAS_HALFWORD_INSTRUCTIONS */

	/*
	 * Push the 3 stack-based parameters in r1-r3.
	 * They are pushed in order r3, r2, r1
	 */

	STMFD	sp!, {r1-r3}

	LDR	r3, [r0, #SEM_STATE]	/* param2 (r3) = semaphore state */
	MOV	r2, r0			/* param1 (r2) = semId */
	MOV	r1, #3			/* nParam */
	LDR	r0, L$_EVENT_SEMTAKE	/* action = eventId */

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

	ADD	sp, sp, #12		/* strip 3 parameters from stack */

	LDMFD	sp!, {r0,r1,lr}		/* restore semId, timeout and link */

trgSemTakeEvt:
        LDR	r2, L$_trgEvtClass	/* is triggering on? */
	LDR	r2, [r2]
	LDR	r3, L$_TRG_CLASS_3_ON
        AND	r2, r2, r3	
        TEQS	r2, r3