semalib.s

vxworks source code, used for develop vxworks system.

/* semALib.s - VxWorks binary semaphore assembler library */

/* Copyright 1996-2001 Wind River Systems, Inc. */
			
/*
modification history
--------------------
01p,30may02,m_h  semBGive unintended effects of MSREQ (78125)
01o,12mar02,m_h  semTake must return error if called from ISR (74202)
01n,15jan02,m_h  smObjPoolMinusOne typo in .extern
01m,30oct01,pcm  added VxWorks semaphore events
01l,17oct01,t_m  convert to FUNC_LABEL:
01k,11oct01,jb  Enabling removal of pre-pended underscores for new compilers
                 (Diab/Gnu elf)
01j,23nov98,cdp  added big-endian support; fix Thumb instrumentation (not
		 currently used); make Thumb support dependent on ARM_THUMB;
		 optimise for no instrumentation.
01i,09sep98,cjtc completing port for WV20
01h,02sep98,cjtc port to windView 2.0
01g,31jul98,pr   temporarily commentig out WindView code
01f,10feb98,cdp  updated for coding standards.
01e,27oct97,kkk  took out "***EOF***" line from end of file.
01d,15oct97,tam  made semBTake return OK on success.
01c,05sep97,cdp  complete rewrite to facilitate instrumentation.
01b,27may97,jpd  Amalgamated into VxWorks.
01a,05jul96,ams  Written.
*/

/*
DESCRIPTION
This module contains internals to the VxWorks kernel.
These routines have been coded in assembler because they are optimized for
performance.
*/

#define _ASMLANGUAGE
#include "vxWorks.h"
#include "vwModNum.h"
#include "asm.h"
#include "eventLib.h"
#include "semLib.h"
#include "private/semLibP.h"
#include "private/classLibP.h"
#include "private/sigLibP.h"
#include "private/eventP.h"

	.data
	.globl	FUNC(copyright_wind_river)
	.long	FUNC(copyright_wind_river)

#if (defined(PORTABLE))
#define semALib_PORTABLE
#endif

#ifndef semALib_PORTABLE

/* globals */

	.globl	FUNC(semGive)		/* optimized semGive demultiplexer */
	.globl	FUNC(semTake)		/* optimized semTake demultiplexer */
	.globl	FUNC(semBGive)		/* optimized binary semaphore give */
	.globl	FUNC(semBTake)		/* optimized binary semaphore take */
	.globl	FUNC(semQGet)		/* semaphore queue get routine */
	.globl	FUNC(semQPut)		/* semaphore queue put routine */
	.globl	FUNC(semOTake)		/* optimized old semaphore take */
	.globl	FUNC(semClear)		/* optimized old semaphore semClear */
	.globl	FUNC(semEvRsrcSend)	/* semaphore event rsrc send routine */

/* externals */

	.extern	FUNC(_func_sigTimeoutRecalc)
	.extern	FUNC(intCnt)
	.extern	FUNC(kernelState)
	.extern	FUNC(semClass)
	.extern	FUNC(semGiveDefer)
	.extern	FUNC(semGiveTbl)
	.extern	FUNC(semInvalid)
	.extern	FUNC(semTakeTbl)
	.extern	FUNC(smObjPoolMinusOne)
	.extern	FUNC(taskIdCurrent)
	.extern	FUNC(windExit)
	.extern	FUNC(windPendQGet)
	.extern	FUNC(windPendQPut)
#ifdef WV_INSTRUMENTATION
        .extern	FUNC(evtAction)
        .extern	FUNC(wvEvtClass)
        .extern	FUNC(trgEvtClass) 
        .extern	FUNC(_func_evtLogM1)
        .extern	FUNC(_func_trgCheck)
        .extern	FUNC(semInstClass)
#endif /* WV_INSTRUMENTATION */

#if	(ARM_THUMB)
	.extern	FUNC(arm_call_via_r2)
	.extern	FUNC(arm_call_via_r12)
	.extern	FUNC(windPendQGet)
#endif	/* ARM_THUMB */

	.text
	.balign	4
/******************************************************************************/

/*
 * PC-relative-addressable pointers - LDR Rn,=sym was (is?) broken
 * note "_" after "$" to stop preprocessor performing substitution
 */

L$__func_sigTimeoutRecalc:
	.long	FUNC(_func_sigTimeoutRecalc)
L$_intCnt:
	.long	FUNC(intCnt)
L$_kernelState:
	.long	FUNC(kernelState)
L$_semClass:
	.long	FUNC(semClass)
L$_semGiveTbl:
	.long	FUNC(semGiveTbl)
L$_semTakeTbl:
	.long	FUNC(semTakeTbl)
L$_smObjPoolMinusOne:
	.long	FUNC(smObjPoolMinusOne)
L$_taskIdCurrent:
	.long	FUNC(taskIdCurrent)
L$_errno:
	.long	FUNC(errno)

#if	(ARM_THUMB)
L$_windPendQGet:
	.long	FUNC(windPendQGet)
#endif	/* ARM_THUMB */

#ifdef WV_INSTRUMENTATION
L$_evtAction:
	.long	FUNC(evtAction)
L$_wvEvtClass:
	.long	FUNC(wvEvtClass) 
L$_trgEvtClass:
	.long	FUNC(trgEvtClass) 
L$_semInstClass:
	.long	FUNC(semInstClass)
L$__func_evtLogM1:
	.long	FUNC(_func_evtLogM1)
L$__func_trgCheck:
	.long	FUNC(_func_trgCheck)
L$_EVENT_SEMGIVE:
	.long	EVENT_SEMGIVE
L$_EVENT_SEMTAKE:
	.long	EVENT_SEMTAKE
L$_EVENT_OBJ_SEMTAKE:
	.long	EVENT_OBJ_SEMTAKE
L$_TRG_CLASS_2_ON:
	.long	TRG_CLASS_2_ON
L$_TRG_CLASS_3_ON:
	.long	TRG_CLASS_3_ON
#endif /* WV_INSTRUMENTATION */

/*******************************************************************************
*
* semGive - give a semaphore
*
* This routine performs the give operation on a specified semaphore.
* Depending on the type of semaphore, the state of the semaphore and of the
* pending tasks may be affected.  The behavior of semGive() is discussed
* fully in the library description of the specific semaphore type being used.
*
* RETURNS: OK, or ERROR if the semaphore ID is invalid.

* STATUS semGive
*	(
*	SEM_ID semId	/@ semaphore ID to give @/
*	)

*/

FUNC_LABEL(semGive)

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

	/* Semaphore is local */

#ifdef WV_INSTRUMENTATION

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

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

	/* instrumentation currently disabled */

resumeSemGive:

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


	/* if in kernelState, defer */

	LDR	r1, L$_kernelState	/* get addr of kernelState */
	LDR	r1, [r1]		/* load value of kernelState */
	TEQS	r1, #0			/* in kernel state? */
	BNE	FUNC(semGiveDefer)		/* branch if so (r0 = semId) */
	
	/* Give the semaphore now, with fast route for binary semaphores */

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

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

/*******************************************************************************
*
* semBGive - give a binary semaphore
*
* Gives the semaphore.  If a higher priority task has already taken
* the semaphore (so that it is now pended waiting for it), that task
* will now become ready to run, and preempt the task that does the semGive().
* If the semaphore is already full (it has been given but not taken) this
* call is essentially a no-op.
*
* RETURNS: OK, or ERROR if the semaphore ID is invalid.

* STATUS semBGive
*	(
*	SEM_ID semId	/@ semaphore ID to give @/
*	)

*/

FUNC_LABEL(semBGive)
	/* LOCK INTERRUPTS */

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

	/* check validity of semaphore */

	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, semInstClass? */
	TEQNES	r12, r2
#endif /* WV_INSTRUMENTATION */
	BNE	semIsInvalidUnlock	/* branch if invalid */

	/* semaphore is valid */

	LDR	r1, [r0, #SEM_Q_HEAD]	/* update semaphore */
	LDR	r2, [r0, #SEM_STATE]	/* old semOwner */
	STR	r1, [r0, #SEM_STATE]	/* new semOwner */
	TEQS	r1, #0			/* empty? */
	BNE	FUNC(semQGet)		/* if not, get from queue */

	LDR	r12, [r0, #SEM_EVENTS_TASKID]
	TEQS	r12, #0			/* Test the semId->events.taskId and */
	TEQNE	r2, #0			/* previous semOwner against NULL. */
	
	BNE     FUNC(semEvRsrcSend)

	MSR	cpsr, r3		/* Return immediately if either one */
	MOV	r0, #OK			/* was NULL (no state change). */ 
	MOV	pc, lr			/* Otherwise fall through to */
					/* FUNC_LABEL(semEvRsrcSend) to send */
					/* semaphore event. */

/******************************************************************************
*
* semEvRsrcSend - send semaphore event to a task
*
* This sub-routine makes a call to eventRsrcSend() if there has been a state
* change on the semaphore, and semId->events.taskId is not NULL.  The tests for
* determining whether to a state change has occurred are done before this 
* sub-routine is executed.  Note the current register usage.
*
* r0:	semId
* r1:	Can Change
* r2:	Can Change
* r3:	previous cpsr
* r12:	semId->events.taskId
*/
FUNC_LABEL(semEvRsrcSend)
	LDR	r1, L$_kernelState
	MOV	r2, #1			/* ENTER KERNEL */
	STR	r2, [r1]
	MSR	cpsr, r3		/* UNLOCK INTERRUPTS */

	STMFD	sp!, {r4-r6, lr}	/* Save r4-r6, and link register */
	MOV	r4, r0			/* r4: semId */
	LDR	r6, L$_errno		/* r6: &errno */
	LDR	r5, [r6]		/* r5: errno */

	LDR	r1, [r0, #SEM_EVENTS_REGISTERED] /* semId->events.registered */
	MOV	r0, r12			/* semId->events.taskId */

	BL	FUNC(eventRsrcSend)	/* eventSend (r0, r1) */

	/*
	 * "if (evSendStatus && !(semOptions & EVENT_SEND_ERROR_NOTIFY))"
	 * can be condensed to one test condition.  This is because
	 * evSendStatus is either OK (0), or ERROR (-1).  AND'ing evSendStatus
	 * with semOptions will either clear the EVENT_SEND_ERROR_NOTIFY bit
	 * if evSendStatus was OK, or leave the bit unchanged if evSendStatus
	 * was ERROR.
	 */

	LDRB	r2, [r4, #SEM_OPTIONS]
	AND	r1, r0, r2
	TST	r1, #SEM_EVENTSEND_ERR_NOTIFY

	MOVNE	r3, #ERROR			/* covers <retStatus> */
	MOVEQ	r3, #OK

	/* Load S_eventLib_EVENTSEND_FAILED into r5 if <retStatus> == ERROR */

	MOV	r2, #(S_eventLib_EVENTSEND_FAILED >> 16)
	MOVNE	r5, #(S_eventLib_EVENTSEND_FAILED & 65535)
	ORRNE	r5, r5, r2, LSL #16		/* covers <errno> */
	
	/*
	 * "if (evSendStatus || !(evtOptions & EVENTS_SEND_ONCE))"
	 * can be condensed to one test condition.  This is because
	 * evSendStatus is either OK (0), or ERROR (-1).  OR'ing evSendStatus
	 * with evtOptions will either set the EVENTS_SEND_ONCE bit if
	 * evSendStatus was ERROR, or leave the bit unchanged if evSendStatus
	 * was OK.
	 */

	LDRB	r2, [r4, #SEM_EVENTS_OPTIONS]
	ORR	r1, r0, r2
	TST	r1, #EVENTS_SEND_ONCE

	MOV	r0, #0				/* Clear semId->events.taskId */
	STRNE	r0, [r4, #SEM_EVENTS_TASKID]	/* if above test evaluated to */
						/* TRUE. */

	MOV	r4, r3				/* Save <retStatus> */
	BL	FUNC(windExit)			/* KERNEL EXIT */

	MOV	r0, r4				/* return <retStatus> */
	STR	r5, [r6]			/* Update errno */
	LDMFD	sp!, {r4-r6, pc}		/* Restore r4-r6, and return */
	
	/* NEVER FALL THROUGH */

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

semGiveGlobal:
	/*
	 * semaphore is shared
	 * r0-> semaphore
	 */

	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 	r1, [r0, #SEM_TYPE]	/* get sem type in network byte order */
#else
	LDRB	r1, [r0, #SEM_TYPE+3]	/* get sem type in network byte order */
#endif
	AND	r1, r1, #SEM_TYPE_MASK	/* index into table */

semGiveCallViaTable:
	/*
	 * r0-> semaphore (parameter for semaphore function)
	 * r1 = index into semGiveTbl
	 * lr = return address
	 */

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

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