inssub.c

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

//*************************************************************************
//
//  Copyright (C) SEIKO EPSON CORP. 1997
//  All Rights Reserved
//
//  Filename : inssub.c
//  Function : Inside Subroutine Group
//  Revision :
//          2001/03/14  Y.Taka      start
//
//*************************************************************************
#include "Ros33.h"
#include "internal.h"


//****************************************************************************
//***    Massage Buffer Subroutine                                         ***
//****************************************************************************


/*
 *  Message buffer pointer information
 *  
 * int ros_put_mbf()
 *  
 *  T_MSGBUFCB* pMsgbufcb   pointer to message buffer control block
 *  VP          msg         pointer to message 
 *  INT         msgsz       message size
 *
 *  return value
 *      1   buffer copy success
 *      -1  buffer is full
 *      -2  buffer size is "0" 
 *
 *
 *
 * bufst   head          tail     bufend 
 *  +-------+------+-------+--------+
 *  | empty | msg1 | msg2  | empty  |
 *  +-------+------+-------+--------+
 *
 *
 *  Message structure
 *  +----------+--------------+
 *  | msg size | message      |
 *  +----------+--------------+
 *
 */

int ros_put_mbf ( T_MSGBUFCB* pMsgbufcb, VP msg, INT msgsz)
{
    VP      bufst;  // message buffer start address
    VP      bufend; // message buffer end address
    VP      head;   // first message start address in queue
    VP      tail;   // last message end address in queue

    int     empty_size;
    int     i, temp;
    
    bufst = pMsgbufcb->bufst;
    bufend  = pMsgbufcb->bufend;
    head = pMsgbufcb->head;
    tail = pMsgbufcb->tail;

    if(bufst == bufend)
        return -2;  /* size 0 message buffer */

    if((tail == head) && (pMsgbufcb->ubEmpty == 0))
        return -1;  /* buffer is full */

    if(tail >= head) {
        empty_size = (int)bufend - (int)bufst;
        empty_size -= (int)tail - (int)head;
    } else {
        empty_size = (int)head - (int)tail;
    }

    if(msgsz+4 > empty_size)    // +4 is header size
        return -1;  /* buffer is full */

    /* save msg size start 4 byte */
    temp = msgsz;
    i = 4;
    while(i--) {
        *(char*)tail = temp;  
        ((char*)tail)++; //GFD
        if(tail >= bufend)
            tail = bufst;
        temp >>= 8;
    }
    
    while(msgsz--) {
        *((char*)tail)++ = *((char*)msg)++;
        if(tail >= bufend)
            tail = bufst;
    }

    /* save current tail */
    pMsgbufcb->tail = tail;

    pMsgbufcb->ubEmpty = 0; // buffer is not empty

    return 1;   /* OK */
}


/*
 * int ros_get_mbf()
 *
 *  T_MSGBUFCB* pMsgbufcb   pointer to message buffer control block
 *  VP          msg         pointer to message (copy message to this buffer)
 *
 *  return value
 *      normal  size of message (byte)
 *      -1      buffer is empty
 *      -2      buffer size is "0" 
 */
int ros_get_mbf(T_MSGBUFCB* pMsgbufcb, VP msg )
{
    VP      bufst;  // message buffer start address
    VP      bufend; // message buffer end address
    VP      head;   // first message start address in queue
    VP      tail;   // last message end address in queue

    int     i, temp;
    int     msgsz;
    
    bufst = pMsgbufcb->bufst;
    bufend  = pMsgbufcb->bufend;
    head = pMsgbufcb->head;
    tail = pMsgbufcb->tail;

    if(bufst == bufend)
        return -2;  /* size 0 message buffer */

    if(pMsgbufcb->ubEmpty == 1)
        return -1;  /* buffer is empty */

    /* read msg size start 4 byte */
    temp = (*(unsigned char*)head)++;
    if(head >= bufend) head = bufst;
    msgsz = temp;

    temp = (*(unsigned char*)head)++;
    if(head >= bufend) head = bufst;
    temp <<= 8;
    msgsz |= temp;

    temp = (*(unsigned char*)head)++;
    if(head >= bufend) head = bufst;
    temp <<= 16;
    msgsz |= temp;

    temp =( *(unsigned char*)head)++; //GFD
    if(head >= bufend) head = bufst;
    temp <<= 24;
    msgsz |= temp;


    i = msgsz;
    while(i--) {
        *((char*)msg)++ = *((char*)head)++;
        if(head >= bufend)
            head = bufst;
    }
    
    /* save current head */
    pMsgbufcb->head = head;

    if(head == tail)
        pMsgbufcb->ubEmpty = 1;
    
    return msgsz;   /* OK */
}


//****************************************************************************
//***    Memory pool Subroutine                                            ***
//****************************************************************************

#define TBL_WIDTH  8

/* Control Table */
typedef struct { UW ulAddrTbl;
                 UW ulSizeTbl;
       } sTable ;


//*************************************************************************
//  Shift Up of Row data 
//  [Parameter]  UW    ulAddrTmp    shift start address(upper)
//               INT   iRow         shift row number
//  [Output]     none
//*************************************************************************
static void sft_uprow( UW ulAddrTmp, INT iRow )
{
    sTable   *stTblPtr;
    int      i;

    stTblPtr =(sTable *)(ulAddrTmp - TBL_WIDTH );

    for( i = 0; i < iRow; i++, stTblPtr++ )
    {
        stTblPtr->ulAddrTbl = (stTblPtr+1)->ulAddrTbl;
        stTblPtr->ulSizeTbl = (stTblPtr+1)->ulSizeTbl;
    }
    return;
}


//*************************************************************************
//  Shift Down of Row data 
//  [Parameter]  UW    ulAddrTmp    shift start address(botom)
//               INT   iRow         shift row number
//  [Output]     none
//*************************************************************************
static void sft_downrow( UW ulAddrTmp, INT iRow )
{
    sTable   *stTblPtr;
    int      i;

    stTblPtr =(sTable *)(ulAddrTmp - TBL_WIDTH );

    for( i = 0; i < iRow; i++, stTblPtr-- )
    {
        (stTblPtr+1)->ulAddrTbl = stTblPtr->ulAddrTbl;
        (stTblPtr+1)->ulSizeTbl = stTblPtr->ulSizeTbl;
    }
    return;
}


//*************************************************************************
//  Memory Allocate of MemoryPool(Variable/Fixed-size)
//  [Parameter]  T_MPLCB* pMplcb    MemoryPool control block address
//               VP      *p_blk       get block address(by output)
//               INT      blksz     get block size
//  [Output]     0        success   (allocate address set to blk)
//               -1       failure   (memory full)
//*************************************************************************
int ros_mem_alloc( T_MPLCB* pMplcb,VP *p_blk,INT blksz)
{
    sTable   *stTblPtr;
    UW       ulSizeWk;
    UW       ulAddrWk;
    UW       ulAddrTmp;

    int      iCount, iRow;


    stTblPtr = (sTable *)((VP*)pMplcb->EndAlloc - TBL_WIDTH ); //GFD

    if (((VP*)pMplcb->TblPtr - (VP*)pMplcb->NxtAlcp ) < TBL_WIDTH ){
        return -1;                                               /* can not allocate */
    }
    /* table check, to botom --------------------------------*/
    for ( iCount=0; iCount < pMplcb->ulRow; iCount++, stTblPtr-- )
    {
        /* found free area */
        if (( stTblPtr->ulSizeTbl & 0x80000000 ) == 0 )
        {
            /* check memory space */
            if (stTblPtr->ulSizeTbl >= blksz)
            {
                ulSizeWk = stTblPtr->ulSizeTbl - blksz;          /* rest memory size */
                stTblPtr->ulSizeTbl = blksz | 0x80000000;        /* new size         */
                ulAddrWk = stTblPtr->ulAddrTbl;                  /* allocate address */

                /* regist, rest memory size */
                /* data size is not zero ----------------------------*/
                if ( ulSizeWk > 0 )
                {
                    ulAddrTmp = (UW)pMplcb->TblPtr;
                    iRow = pMplcb->ulRow - iCount - 1;
                    sft_uprow( ulAddrTmp, iRow  );               /* shift up Row */

                    stTblPtr--;
                    stTblPtr->ulSizeTbl = ulSizeWk;              /* entry, rest size */
                    stTblPtr->ulAddrTbl = ulAddrWk + blksz;      /* entry, rest address */

                    (VP*)pMplcb->TblPtr -= TBL_WIDTH;		//GFD
                    pMplcb->ulRow ++;
                }
                *((UW*)p_blk) = ulAddrWk;                          /* set to blk */
                return 0;                                        /* success */
            }
        }
        /* not found, continue next row line */
    }

    /* new add, table data ------------------------------------------*/
    /* check free area */
    //GFD
    if (((VP*)pMplcb->TblPtr -(VP*) pMplcb->NxtAlcp ) < (blksz + TBL_WIDTH )){
        return -1;                                               /* memory full */
    }
    /* entry, new row line */
    ulAddrWk = (UW)pMplcb->NxtAlcp;
    stTblPtr->ulAddrTbl = ulAddrWk;                              /* allocate address */
    stTblPtr->ulSizeTbl = blksz | 0x80000000;                    /* new size         */

    (VP*)pMplcb->NxtAlcp += blksz;		//GFD
    (VP*)pMplcb->TblPtr -= TBL_WIDTH;  //GFD
    pMplcb->ulRow++;

    *((UW*)p_blk) = ulAddrWk;                                      /* set to blk */
    return 0;                                                    /* success */
}


//*************************************************************************
//  Memory Block Free of MemoryPool(Variable/Fixed-size)
//  [Parameter]  T_MPLCB* pMplcb    MemoryPool control block address
//               VP       blk       release block address(by output)
//  [Output]     0        success   (released block)
//               -1       failure   (not found of block address)
//*************************************************************************
int ros_mem_free(T_MPLCB* pMplcb, VP blk)
{
    sTable   *stTblPtr;
    UW       ulSizeWk;
    UW       ulAddrTmp;

    int iLoop, iRow;

    stTblPtr = (sTable *)((VP*)pMplcb->EndAlloc - TBL_WIDTH );

    /* do search, to botom */
    for ( iLoop=0; iLoop < pMplcb->ulRow; iLoop++, stTblPtr--)
    {
        /* found block address */
        if (blk == (VP)stTblPtr->ulAddrTbl)
        {
            /* clear flag */
            ulSizeWk = stTblPtr->ulSizeTbl & 0x7fffffff;
            stTblPtr->ulSizeTbl = ulSizeWk;                      /* flag clear & store */

            /* if last row data */
            if (iLoop == pMplcb->ulRow - 1)
            {
                pMplcb->ulRow--;                                 /* free last row data */
                (VP*)pMplcb->TblPtr += TBL_WIDTH; //gfd
                (VP*)pMplcb->NxtAlcp -= ulSizeWk;  //gfd

                /* check previouse row data */
                stTblPtr++;
                if (( iLoop != 0) && (stTblPtr->ulSizeTbl & 0x80000000) == 0)
                {
                    ulSizeWk = stTblPtr->ulSizeTbl;
                    pMplcb->ulRow--;                        /* free previouse row data */ 
                    (VP*)pMplcb->TblPtr += TBL_WIDTH;   //gfd
                    (VP*)pMplcb->NxtAlcp -= ulSizeWk;
                }
            }
            else {
                /* marge & shift */
                /* check previouse row data */
                if (( iLoop != 0 ) && ((stTblPtr+1)->ulSizeTbl & 0x80000000) == 0 )
                {
                    ulAddrTmp = (UW)stTblPtr;
                    stTblPtr++;
                    ulSizeWk = ulSizeWk + stTblPtr->ulSizeTbl;  /* add rest size */
                    stTblPtr->ulSizeTbl = ulSizeWk;             /* add rest size */

                    iRow = pMplcb->ulRow - iLoop - 1;
                    sft_downrow( ulAddrTmp, iRow );              /* shift down Row */

                    pMplcb->ulRow--;
                    (VP*)pMplcb->TblPtr += TBL_WIDTH;//gfd
                    iLoop--;
                }

                /* check next row data */
                if (((stTblPtr-1)->ulSizeTbl & 0x80000000) == 0 )
                {
                    stTblPtr->ulSizeTbl = ulSizeWk + ((stTblPtr-1)->ulSizeTbl);
                                                                 /* add rest size */
                    iLoop++;
                    stTblPtr--;
                    iRow = pMplcb->ulRow - iLoop - 1;
                    ulAddrTmp = (UW)stTblPtr;
                    sft_downrow( ulAddrTmp, iRow );              /* shift down Row */

                    pMplcb->ulRow--;
                    (VP*)pMplcb->TblPtr += TBL_WIDTH;  //gfd

                }
            }
            return 0;                                            /* success */
        }
        /* not found, continue next row line */
    }

    return -1;                                                   /* not found */
}