semapho.c

最近在國外網站抓到的作業系統 以Arm為基礎去開發的

//*************************************************************************
//
//  Copyright (C) SEIKO EPSON CORP. 1997
//  All Rights Reserved
//
//  Filename : semaphore.c
//  Function : Semaphore function
//  Revision :
//          1997/08/01  H.Matsuoka  start
//          1999/08/27  H.Matsuoka  BUG FIX(critical section)
//          1999/11/25  H.Matsuoka  Add isig_sem
//          2000/02/23  H.Matsuoka  Fix suspend status problem
//          2000/11/30  Y.Taka      Add Enter/Return_critical_section
//          2001/01/22  Y.Taka      Add twai_sem()
//          2001/03/08  Y.Taka      Change critical section
//          2001/04/24  Y.Taka      Add semaphore max counter
//
//*************************************************************************
#include "Ros33.h"
#include "internal.h"

//****************************************************************************
//***    3.3 Synchronization and Communication Functions                   ***
//****************************************************************************

//********************************************************************
//  Signal Semaphore
//  [Parameters]
//      ID  semid   SemaphoreID
//  [Return Parameters]
//      ER  ercd    ErrorCode
//********************************************************************
ER sig_sem
    (
    ID  semid
    )
{
    T_SMPHCB*   pSmphcb;
    T_TSKCB*    pTskcb;
	UW psr;
	
#ifndef NO_ERROR_CHECK    
    if(semid <= 0)
    {
        return E_ID;
    }
    
    if(semid > SMPH_NUM)
    {
        return E_NOEXS;
    }
#endif

	psr = ent_cri();
	
    pSmphcb = &g_sSmphcb[semid-1];

#ifndef NO_ERROR_CHECK
    if (pSmphcb->ubSmphCnt >= pSmphcb->ubSmphMax)
    {
    	ret_cri(psr);
        return E_QOVR;
    }
#endif

    if(pSmphcb->pNxtTsk != (UW*)pSmphcb) {      // exist semaphore wait task
        pTskcb = (T_TSKCB*)pSmphcb->pNxtTsk;

        pTskcb->ubStatus &= ~TTS_WAI;
        pTskcb->ubWaitStat &= ~TTW_SEM;

        if(pTskcb->ubIntinfo == 0)          /* interrupt information */
        {
            ((T_SAVEDREG_MIN*)(pTskcb->uwSP))->returnvalue = E_OK;//GFD
        }
            
        /* clear time out task link */
        vfnDelInitMember((T_NODE*)&(pTskcb->pNxtTmoTsk));

        vfnDelAddMember((T_NODE*)&g_sReadyQueue[pTskcb->bPriority], 
                       (T_NODE*)pTskcb);
        int_dispatch();         // dispatch & disable interrupt
    } 
    else 
    {
        pSmphcb->ubSmphCnt++;
    }


	ret_cri(psr);
#ifndef NO_RETURN_VALUE
    return E_OK;
#endif
}


//********************************************************************
//  Wait on Semaphore
//  [Parameters]
//      ID  semid   SemaphoreID
//      TMO tmout   Timeout
//  [Return Parameters]
//      ER  ercd    ErrorCode
//********************************************************************
ER twai_sem
    (
    ID  semid,
    TMO tmout
    )
{
    T_SMPHCB*   pSmphcb;
    T_TSKCB*    pTskcb;
	UW psr;


#ifndef NO_ERROR_CHECK    
    if((g_ubIntNestCnt != 0)    // issued from task-independent portions
    || (g_ubSysStat & TSS_DDSP)) // task in dispatch disabled state
    {   
        return E_CTX;
    }

    if(semid <= 0)
    {
        return E_ID;
    }
    
    if(semid > SMPH_NUM)
    {
        return E_NOEXS;
    }
    if(tmout <= -2)
    {
        return E_PAR;
    }
#endif

	psr = ent_cri();
	
    pSmphcb = &g_sSmphcb[semid-1];
    
    if(pSmphcb->ubSmphCnt == 0) 
    {
        if (tmout == TMO_POL)
        {
        	ret_cri(psr);
            return E_TMOUT;
        }

        // wait status

        g_pCurTsk->ubStatus |= TTS_WAI;  
        g_pCurTsk->ubWaitStat |= TTW_SEM;
        
        vfnDelAddMember((T_NODE*)pSmphcb, (T_NODE*)g_pCurTsk);
        
        /* set time out */
        if(tmout != TMO_FEVR) {
            g_pCurTsk->utime = g_sSysTime.utime;
            if (g_sSysTime.ltime > g_sSysTime.ltime + tmout)
                g_pCurTsk->utime +=  1;
            g_pCurTsk->ltime = g_sSysTime.ltime + tmout;
            vfnAddTimer((T_NODE*)&(g_pCurTsk->pNxtTmoTsk));
        }

        int_dispatch();         // dispatch & disable interrupt
        ret_cri(psr);
        MOV_R1TOR0;		// mov r1(from the returnvalue in the stack) to ro (in asm)		
        return;                 // return value is E_OK or E_RLWAI
        
    } else {
        pSmphcb->ubSmphCnt--;
    }
    
	ret_cri(psr);
#ifndef NO_RETURN_VALUE
    return E_OK;
#endif
}