semalib.s

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

/* semALib.s - internal VxWorks kernel semaphore assembler library */

/* Copyright 1984-2001 Wind River Systems, Inc. */
	.data
	.globl	copyright_wind_river

/*
 * This file has been developed or significantly modified by the
 * MIPS Center of Excellence Dedicated Engineering Staff.
 * This notice is as per the MIPS Center of Excellence Master Partner
 * Agreement, do not remove this notice without checking first with
 * WR/Platforms MIPS Center of Excellence engineering management.
 */

/*
modification history
--------------------
02q,16oct02,agf  check for semID being NULL (spr 68917)
                 modify errno handling of semTake from ISR (spr 73802)
02p,12nov01,pcm  closed open-ended comment inside semEvRsrcSend ()
02o,30oct01,pcm  added VxWorks semaphore events
02n,01aug01,mem  Diab integration.
02m,16jul01,ros  add CofE comment
02l,12jun01,mem  Update for new coding standard.
02k,19jun00,dra  work around 5432 branch bug
02j,10sep99,myz  reworked last mod.
02i,07sep99,myz  added mips16 support.
02h,19jan99,dra	 added CW4000, CW4011, V4100, VR5000 and VR5400 support.
02g,16apr99,pr   saved volatile regs in WindView code (SPR #26766 & 26698)
02f,19aug98,pr   fixed bug for EVENT_SEMTAKE/SEMGIVE (SPR #22183)
02e,16may98,pr   temporarily excluded EVENT_SEMTAKE/SEMGIVE evtlogging
02d,21apr98,pr   fixed problems in EVENT_SEMTAKE/SEMGIVE
02c,16apr98,pr   added WindView 2.0 support
02b,13mar97,kkk  moved .set noreorder line in semQPut(), deleted extra
		 .set noreorder line in semQGet().
02a,14oct96,kkk  added R4650 support.
01z,15sep96,kkk  merge in change from ease. (fixed spr# 7154.)
01y,22feb96,mem  fixed R4000 support.  Was using SW/LW with FRAMEAx() and
		 sw/lw with FRAMERx().
01x,16may95,cd	 rewrote semQPut without noreorder section
		 the noreorder section prevented the kernel from
		 being built -G0 and a missing end of comment
		 led to accusations of bugs in the MIPS assembler
01w,23jul96,pr   added windview instrumentation.
01v,19oct93,cd   added R4000 support and enabled for all MIPS targets.
01u,05nov93,yao  added shared memory support.
01t,29sep93,caf  undid fix of SPR #2359.
01s,07jul93,yao  fixed to preserve parity error bit of status 
		 register (SPR #2359).  changed copyright notice.
01r,08aug92,ajm  corrected stack parameters
01q,30jul92,rrr  changed _sig_timeout_recalc to _func_sigTimeoutRecalc
01p,10jul92,ajm  changed semBTake jump to semTake jump
01o,09jul92,ajm  changed semQPut for new signals
01n,06jul92,ajm  split into sem[CM]ALib.s to increase modularity.
01n,04jul92,jcf  scalable/ANSI/cleanup effort.
01m,26may92,rrr  the tree shuffle
01l,15oct91,ajm   pulled in optimizations
01k,04oct91,rrr   passed through the ansification filter
                   -fixed #else and #endif
                   -changed ASMLANGUAGE to _ASMLANGUAGE
                   -changed copyright notice
01j,01aug91,ajm   removed assembler .set noreorder macros. They tend to screw
		   up assembler
01i,14may91,ajm   ported to MIPS
01h,16oct90,jcf   fixed race of priority inversion in semMGive.
01g,01oct90,dab   changed conditional compilation identifier from
		    HOST_SUN to AS_WORKS_WELL.
01f,12sep90,dab   changed complex addressing mode instructions to .word's
           +lpf     to make non-SUN hosts happy.
01e,27jun90,jcf   optimized version once again.
01d,26jun90,jcf   made PORTABLE for the nonce.
01c,10may90,jcf   added semClear optimization.
01b,23apr90,jcf   changed name and moved to src/68k.
01a,02jan90,jcf   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 "asm.h"
#include "eventLib.h"
#include "objLib.h"
#include "semLib.h"
#include "vwModNum.h"
#include "private/taskLibP.h"
#include "private/semLibP.h"
#include "private/eventP.h"
#include "private/workQLibP.h"
#include "private/eventLibP.h"

/* abstracted from private/semLibP.h (where they should be globally defined) */

#define SEM_CLASS		0x00	/* offset into SEMAPHORE */
#define SEM_SM_TYPE		0x04
		
/* optimized version available for MIPS architecture */

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

#ifndef semALib_PORTABLE

	/* globals */

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

	/* externals */

	.extern	kernelState		/* in kernel ? */
	.extern	taskIdCurrent		/* running task */
	.extern	intCnt			/* interrupt nest counter */
	.extern	semMGiveKernWork	/* what does kern have ? */
	.extern	_func_sigTimeoutRecalc	/* signal timeout routine */
	.extern smObjPoolMinusOne	/* smObj pool local address */
	.extern	errno			/* errno */
	.extern semGiveTbl		/* semaphhore function table */
	.extern semTakeTbl		/* semahphore function table */

	.text
	.set	reorder

#ifdef WV_INSTRUMENTATION

	.extern _func_evtLogTSched	/* timestamp function pointer */
	.extern _func_trgCheck		/* timestamp function pointer */
	.extern _func_evtLogT0		/* timestamp function pointer */
	.extern evtAction	    
	.extern wvEvtClass	    
	.extern trgEvtClass	    

/* 
 * The stack carving macros for WV is redefined here , because we 
 * need to ensure 32 bit register usages, as the kernel is built 
 * with -gp32 (or use 32 bit registers), will make use of "lw"
 * instead of "ld" for R4000.
 *
 * Note that the stack is aligned at 8-byte boundary, per the ABI.
 * And, we must always carve 20 bytes for a0-a4 and ra, regardless whether
 * the callee requires a parameter or not.
 */
#define WV_FRAMESZ(routine)        _##routine##Fsize

#define WV_SETFRAME(routine,nregs) \
        WV_FRAMESZ(routine) = ((((4*4)+4*(1+(nregs))) + 7) & ~7)

/* The location at which to store the return address */

#define WV_FRAMERA(routine) \
        (WV_FRAMESZ(routine)-4)

/* 
 * Locations at which to store arguments passed on stack or
 * locally used registers 
 */

#define WV_FRAMER(routine,regn) \
        ((4*4)+(4*(regn)))
#define WV_FRAMER0(routine) WV_FRAMER(routine,0)
#define WV_FRAMER1(routine) WV_FRAMER(routine,1)
#define WV_FRAMER2(routine) WV_FRAMER(routine,2)
#define WV_FRAMER3(routine) WV_FRAMER(routine,3)
#define WV_FRAMER4(routine) WV_FRAMER(routine,4)
#define WV_FRAMER5(routine) WV_FRAMER(routine,5)
#define WV_FRAMER6(routine) WV_FRAMER(routine,6)
#define WV_FRAMER7(routine) WV_FRAMER(routine,7)
#define WV_FRAMER8(routine) WV_FRAMER(routine,8)
#define WV_FRAMER9(routine) WV_FRAMER(routine,9)
#define WV_FRAMER10(routine) WV_FRAMER(routine,10)
#define WV_FRAMER11(routine) WV_FRAMER(routine,11)
#define WV_FRAMER12(routine) WV_FRAMER(routine,12)
#define WV_FRAMER13(routine) WV_FRAMER(routine,13)

/* Locations at which to store 32bit argument registers */

#define WV_FRAMEA(routine, regn) \
          (WV_FRAMESZ(routine) + 4*(regn))
#define WV_FRAMEA0(routine) WV_FRAMEA(routine,0)
#define WV_FRAMEA1(routine) WV_FRAMEA(routine,1)
#define WV_FRAMEA2(routine) WV_FRAMEA(routine,2)
#define WV_FRAMEA3(routine) WV_FRAMEA(routine,3)

#endif /* WV_INSTRUMENTATION */

/*******************************************************************************
*
* semGiveKern - add give routine to work queue
*
*/

semGiveKern:
	j	semGiveDefer		/* let C rtn defer work and rts */

/*******************************************************************************
*
* semGive - give a semaphore
*
*
*STATUS semGive (semId)
*    SEM_ID semId;		* semaphore id to give 
* 
*/

	.ent	semGive
semGive:
        beq     a0, zero, semNULLRestrict	/* dont allow NULL ref */
	lw	v0, kernelState			/* are we in kernel state? */
	bne	zero, v0, semGiveKern		/* v0 != 0 if we are not */
	andi	t0, a0, 1			/* check if smObj id */
	beq	t0, zero, 1f			/* t0==zero, local semaphore */
	lw	t1, smObjPoolMinusOne		/* load local pool address  */	
	SETFRAME(semGive,0)
	subu	sp, FRAMESZ(semGive)		/* carve stack space */
	addu	a0, a0, t1			/* convert id to local addr */
	SW	ra, FRAMERA(semGive)(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, semGiveTbl			/* load semaphore give table */
	sll	t2, 2				/* scale by size of (FUNCPTR) */
	addu	t3, t2, t3			/* appropriate give table */
	lw	t4, (t3)			/* get function address */
	jal	t4				/* call give func */
	LW	ra, FRAMERA(semGive)(sp)	/* get return address */
	addu 	sp, FRAMESZ(semGive)		/* pop up stack */
	j	ra
1:
#ifdef WV_INSTRUMENTATION
	/* windview instrumentation - BEGIN
	 * semGive level 1 (object status event )
	 */
	lw	t0, evtAction		/* is WV and-or triggering on? */
	beqz	t0, noSemGiveEvt

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

        /* 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, trgCheckSemGive

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

	lhu	t2, SEM_RECURSE(a0)

	/* log event for this object */

	/* 
	 * NOTE: If the following called function changes the number of
	 * parameters passed, then we need to change how we pass arguments
	 * to the function. The following assumes the called function
	 * takes 7 parameters, last two are 0.
	 */

	WV_SETFRAME(semGiveInst,11)
	subu	sp, WV_FRAMESZ(semGiveInst) /* create stack frame */
	sw	ra, WV_FRAMERA(semGiveInst)(sp)	/* save ra */
	sw	a0, WV_FRAMEA0(semGiveInst)(sp)	/* and a0 */
	sw	t2, WV_FRAMER0(semGiveInst)(sp)	/* pass t2 to 5th arg of func */
	sw	zero, WV_FRAMER1(semGiveInst)(sp) /* pass 0 to 6th arg */
	sw	zero, WV_FRAMER2(semGiveInst)(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. No need for v1 either.
	 */

	sw	v0, WV_FRAMER3(semGiveInst)(sp) 	/* save v0 */
	sw	t1, WV_FRAMER4(semGiveInst)(sp) 	/* save t1 */
	sw	t3, WV_FRAMER5(semGiveInst)(sp) 	/* save t3 */
	sw	t5, WV_FRAMER6(semGiveInst)(sp) 	/* save t5 */
	sw	t6, WV_FRAMER7(semGiveInst)(sp) 	/* save t6 */
	sw	t7, WV_FRAMER8(semGiveInst)(sp) 	/* save t7 */
	sw	t8, WV_FRAMER9(semGiveInst)(sp) 	/* save t8 */
	sw	t9, WV_FRAMER10(semGiveInst)(sp) 	/* save t9 */
	
	lw	a3, SEM_STATE(a0)		/* put sem state in 3rd arg */
	move	a2, a0				/* a2 = semID, 2nd arg */
	li	a1, 3				/* # of args passed to func */
	li	a0, EVENT_SEMGIVE		/* event ID, in arg0 */
	jal	t0				/* call routine */

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

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

	lw	t2, WV_FRAMER0(semGiveInst)(sp)
	lw	a0, WV_FRAMEA0(semGiveInst)(sp)
	lw	ra, WV_FRAMERA(semGiveInst)(sp)
	addu    sp, WV_FRAMESZ(semGiveInst)

trgCheckSemGive:

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

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

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

	/* No need to save t0 t4, since they are trashed above. 
	   Non need to save v1 either (trashed below).
	 */

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

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

        move    a3, a0  		
	move	a2, a0

	li	a0, EVENT_SEMGIVE
        li      a1, TRG_CLASS3_INDEX
        lw      t1, _func_trgCheck
        jal     t1                              /* call trgCheck routine */

	/* now restore  */

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

	lw	t2, WV_FRAMER1(semGiveTrg)(sp)
	lw	t1, WV_FRAMER0(semGiveTrg)(sp)
	lw	a0, WV_FRAMEA0(semGiveTrg)(sp)
	lw	ra, WV_FRAMERA(semGiveTrg)(sp)
	addu    sp, WV_FRAMESZ(semGiveTrg)