mempol.c

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

//*************************************************************************
//
//  Copyright (C) SEIKO EPSON CORP. 2000
//  All Rights Reserved
//
//  Filename : mempol.c
//  Function : Memory Pool Management functions
//  Revision :
//          2000/12/05  Y.Taka      start
//          2001/01/22  Y.Taka      Replace vfnAddTimer
//          2001/03/08  Y.Taka      Change critical section
//
//*************************************************************************
#include "Ros33.h"
#include "internal.h"


//********************************************************************
//  Get Variable-size Memory Block 
//  [Parameters]
//      VP     *p_blk   Start Address of Memory Block
//      ID      mplid   Variable-size MemoryPoolID
//      INT     blksz   Get Block Size(in bytes)
//      TMO     tmout   Timeout
//  [Return Parameters]
//      ER      ercd    ErrorCode
//********************************************************************
ER tget_blk
    (
    VP     *p_blk,
    ID      mplid,
    INT     blksz,
    TMO     tmout
    )
{
    T_TSKCB*    pTskcb;
    T_MPLCB*    pMplcb;
    
#ifndef NO_ERROR_CHECK    
    if((g_ubIntNestCnt != 0)        // issued from task-independent portions
    || (g_ubSysStat & TSS_DDSP))    // task in dispatch disabled state
    {   
        if (tmout != TMO_POL)
            return E_CTX;
    }

    if(mplid <= 0)
    {
        return E_ID;
    }
    
    if(mplid > MPLBLK_NUM)
    {
        return E_NOEXS;
    }
    
    if(blksz <= 0)
    {
        return E_PAR;
    }
    
    if(tmout <= -2)
    {
        return E_PAR;
    }
#endif
    
    pMplcb = &g_sMplcb[mplid-1];
    
#ifndef NO_ERROR_CHECK    
    if(pMplcb->StartAlloc == 0)
    {
        return E_NOEXS;
    }

    blksz = (blksz + 3) & 0xfffffffc;
    if((blksz + 8) >(INT) ((VP*)pMplcb->EndAlloc -(VP*)pMplcb->StartAlloc))	//GFD
    {
        return E_PAR;
    }
#endif

    //memory pool allocate
    if(ros_mem_alloc(pMplcb, p_blk, blksz) == 0)
    {
        return E_OK;
    }

    /* following memory pool full case. wait until pool is empty. */

    if (tmout == TMO_POL)
    {
        return E_TMOUT;
    }

    g_pCurTsk->ubStatus |= TTS_WAI;  
    g_pCurTsk->ubWaitStat |= TTW_MPL;

    g_pCurTsk->pMplblk = p_blk;
    g_pCurTsk->blksize = blksz;

    vfnDelAddMember((T_NODE*)&(pMplcb->pNxtTsk), 
                   (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
    MOV_R1TOR0;		// mov r1(from the returnvalue in the stack) to ro (in asm)
    return;         // return value is E_OK or E_RLWAI
                    // wakeup task set return value. 
}



//********************************************************************
//  Release Variable-size Memory Block
//  [Parameters]
//      ID      mplid   Variable-size MemorypoolID
//      VP      blk     Start Address of Memory Block
//  [Return Parameters]
//      ER      ercd    ErrorCode
//********************************************************************
ER rel_blk
    (
    ID      mplid, 
    VP      blk
    )
{
    T_TSKCB*    pTskcb;
    T_TSKCB*    pTskcb2;
    T_MPLCB*    pMplcb;

#ifndef NO_ERROR_CHECK    
    if(mplid <= 0)
    {
        return E_ID;
    }
    
    if(mplid > MPLBLK_NUM)
    {
        return E_NOEXS;
    }
#endif

    pMplcb = &g_sMplcb[mplid-1];

#ifndef NO_ERROR_CHECK    
    if(pMplcb->StartAlloc == 0)
    {
        return E_NOEXS;
    }

    if(blk < pMplcb->StartAlloc || blk >= pMplcb->EndAlloc)
    {
        return E_PAR;
    }
#endif

    //free memory pool
    if(ros_mem_free(pMplcb, blk) != 0)
    {
        return E_PAR;
    }

    // released memory block, search of waiting queue

    if(pMplcb->pNxtTsk == (UW*)&(pMplcb->pNxtTsk)) {

        // block space wait task dose not exist 

        return E_OK;

    } 

    // block space wait task exist (get task wait) 

    pTskcb = (T_TSKCB*)pMplcb->pNxtTsk;

    do {
        if(ros_mem_alloc(pMplcb, pTskcb->pMplblk, pTskcb->blksize) == 0) {

            /* wakeup TTW_MPL wait task */
            pTskcb->ubStatus &= ~TTS_WAI;  
            pTskcb->ubWaitStat &= ~TTW_MPL;

            /* set return value for wait task */
            ((T_SAVEDREG_MIN*)pTskcb->uwSP)->r[10] = E_OK;//GFD


            /* save prv task */
            pTskcb2 = (T_TSKCB*)(pTskcb->pPrvTsk);

            /* clear time out task link */
            vfnDelInitMember((T_NODE*)&(pTskcb->pNxtTmoTsk));

            /* add task to ready queue */
            vfnDelAddMember((T_NODE*)&g_sReadyQueue[pTskcb->bPriority],(T_NODE*)pTskcb);

            /* restore prv task pointer */
            pTskcb = pTskcb2;
        }
        pTskcb = (T_TSKCB*)(pTskcb->pNxtTsk);

    } while(pTskcb != (T_TSKCB*)&(pMplcb->pNxtTsk));

    int_dispatch();         // dispatch & disable interrupt
    return E_OK;
}


//********************************************************************
//  Get Fixed-size Memory Block 
//  [Parameters]
//      VP     *p_blf     Start Address of Memory Block
//      ID      mpfid   Fixed-size MemoryPoolID
//      TMO     tmout   Timeout
//  [Return Parameters]
//      ER      ercd    ErrorCode
//********************************************************************
ER tget_blf
    (
    VP     *p_blf,
    ID      mpfid,
    TMO     tmout
    )
{
    T_TSKCB*    pTskcb;
    T_MPLCB*    pMpfcb;
    
    INT         blfsz;

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

    if(mpfid <= 0)
    {
        return E_ID;
    }
    
    if(mpfid > MPFBLK_NUM)
    {
        return E_NOEXS;
    }
    
    if(tmout <= -2)
    {
        return E_PAR;
    }
#endif
    
    pMpfcb = &g_sMpfcb[mpfid-1];
    
#ifndef NO_ERROR_CHECK    
    if(pMpfcb->StartAlloc == 0)
    {
        return E_NOEXS;
    }
#endif

    blfsz = pMpfcb->ulBlksz;

    //memory pool allocate
    if(ros_mem_alloc(pMpfcb, p_blf, blfsz) == 0)
    {
        return E_OK;
    }

    /* following memory pool full case. wait until pool is empty. */

    if (tmout == TMO_POL)
    {
        return E_TMOUT;
    }

    g_pCurTsk->ubStatus |= TTS_WAI;  
    g_pCurTsk->ubWaitStat |= TTW_MPF;

    g_pCurTsk->pMplblk = p_blf;
    g_pCurTsk->blksize = blfsz;

    vfnDelAddMember((T_NODE*)&(pMpfcb->pNxtTsk), 
                   (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
    MOV_R1TOR0;		// mov r1(from the returnvalue in the stack) to ro (in asm)
    return;         // return value is E_OK or E_RLWAI
                    // wakeup task set return value. 
}



//********************************************************************
//  Release Fixed-size Memory Block
//  [Parameters]
//      ID      mpfid   Fixed-size MemorypoolID
//      VP      blf     Start Address of Memory Block
//  [Return Parameters]
//      ER      ercd    ErrorCode
//********************************************************************
ER rel_blf
    (
    ID      mpfid, 
    VP      blf
    )
{
    T_TSKCB*    pTskcb;
    T_TSKCB*    pTskcb2;
    T_MPLCB*    pMpfcb;

#ifndef NO_ERROR_CHECK    
    if(mpfid <= 0)
    {
        return E_ID;
    }
    
    if(mpfid > MPFBLK_NUM)
    {
        return E_NOEXS;
    }
#endif

    pMpfcb = &g_sMpfcb[mpfid-1];

#ifndef NO_ERROR_CHECK    
    if(pMpfcb->StartAlloc == 0)
    {
        return E_NOEXS;
    }

    if(blf < pMpfcb->StartAlloc || blf >= pMpfcb->EndAlloc)
    {
        return E_PAR;
    }
#endif

    //free memory pool
    if(ros_mem_free(pMpfcb, blf) != 0)
    {
        return E_PAR;
    }

    // released memory block, search of waiting queue

    if(pMpfcb->pNxtTsk == (UW*)&(pMpfcb->pNxtTsk)) {

        // block space wait task dose not exist 

        return E_OK;

    } 

    // block space wait task exist (get task wait) 

    pTskcb = (T_TSKCB*)pMpfcb->pNxtTsk;

    do {
        if(ros_mem_alloc(pMpfcb, pTskcb->pMplblk, pTskcb->blksize) == 0) {

            /* wakeup TTW_MPF wait task */
            pTskcb->ubStatus &= ~TTS_WAI;  
            pTskcb->ubWaitStat &= ~TTW_MPF;

            /* set return value for wait task */
            ((T_SAVEDREG_MIN*)(pTskcb->uwSP))->r[10] = E_OK;


            /* save prv task */
            pTskcb2 = (T_TSKCB*)(pTskcb->pPrvTsk);

            /* clear time out task link */
            vfnDelInitMember((T_NODE*)&(pTskcb->pNxtTmoTsk));

            /* add task to ready queue */
            vfnDelAddMember((T_NODE*)&g_sReadyQueue[pTskcb->bPriority],
                            (T_NODE*)pTskcb);

            /* restore prv task pointer */
            pTskcb = pTskcb2;
        }
        pTskcb = (T_TSKCB*)(pTskcb->pNxtTsk);

    } while(pTskcb != (T_TSKCB*)&(pMpfcb->pNxtTsk));

    int_dispatch();         // dispatch & disable interrupt
    return E_OK;

}