void qPriDeltaPut
(
Q_PRI_HEAD *pQPriHead,
Q_PRI_NODE *pQPriNode,
ULONG key
)
{
FAST Q_PRI_NODE *pQNode = (Q_PRI_NODE *) DLL_FIRST (pQPriHead);
pQPriNode->key = key - vxTicks; /* relative queue keeps delta time */
while (pQNode != NULL) /* empty list? */
{
if (pQPriNode->key < pQNode->key)
{
/* We've reached the place in the delta queue to insert this task */
dllInsert (pQPriHead, DLL_PREVIOUS (&pQNode->node),
&pQPriNode->node);
/* Now decrement the delta key of the guy behind us. */
pQNode->key -= pQPriNode->key;
return; /* we're done */
}
pQPriNode->key -= pQNode->key;
pQNode = (Q_PRI_NODE *) DLL_NEXT (&pQNode->node);
}
/* if we fall through, add to end of delta q */
dllInsert (pQPriHead, (DL_NODE *) DLL_LAST (pQPriHead), &pQPriNode->node);
}
STATUS memPartFree(PART_ID partId, char* pBlock)
{
FAST BLOCK_HDR *pHdr;
FAST unsigned nWords;
FAST BLOCK_HDR *pNextHdr;
if(!IS_CLASS(partId, memPartClassId))
{
return (ERROR);
}
if(NULL == pBlock)
{
return (OK);
}
pHdr = BLOCK_TO_HDR(pBlock);
/* TODO take semaphore hear */
semTake(partId->semPartId, WAIT_FOREVER);
if((partId->options & MEM_BLOCK_CHECK)
&& !memPartBlockIsValid(partId, pHdr, FALSE))
{
/* TODO give semaphore hear */
semGive(partId->semPartId);
return (ERROR);
}
nWords = pHdr->nWords;
if(PREV_HDR(pHdr)->free)
{/* the prev hdr is free and than coalesce with it */
pHdr->free = FALSE;
pHdr = PREV_HDR(pHdr);
pHdr->nWords += nWords;
}
else
{
pHdr->free = TRUE;
dllInsert(&partId->freeList, (DL_NODE*)NULL, HDR_TO_NODE(pHdr));
}
/* check to coalesce with the next */
pNextHdr = NEXT_HDR(pHdr);
if(pNextHdr->free)
{
pHdr->nWords += pNextHdr->nWords;
dllRemove(&partId->freeList, HDR_TO_NODE(pNextHdr));
}
/* cannot use pNextHdr->prevHdr=pHdr hear */
NEXT_HDR(pHdr)->prevHdr = pHdr;
partId->curBlocksAlloc--;
partId->curWordsAlloc -= nWords;
/* TODO give sem hear */
semGive(partId->semPartId);
return (OK);
}
void dllAdd
(
DL_LIST *pList, /* pointer to list descriptor */
DL_NODE *pNode /* pointer to node to be added */
)
{
dllInsert (pList, pList->tail, pNode);
}
void qPriListPut(Q_PRI_HEAD* pQPriHead, Q_PRI_NODE* pQPriNode, ULONG key)
{
FAST Q_PRI_NODE* pQNode = (Q_PRI_NODE*)DLL_FIRST(pQPriHead);
pQPriNode->key = key;
while(NULL != pQNode)
{
/* HEAD/number small ----<<<<<----TAIL/number big
The smaller number is, the higher pri is. */
if(key < pQNode->key)
{
dllInsert(pQPriHead, DLL_PREVIOUS(&pQNode->node), &pQPriNode->node);
return;
}
pQNode = (Q_PRI_NODE*)DLL_NEXT(&pQNode->node);
}
/* add at last */
dllInsert(pQPriHead, DLL_LAST(pQPriHead), &pQPriNode->node);
}
void qFifoPut
(
Q_FIFO_HEAD *pQFifoHead,
Q_FIFO_NODE *pQFifoNode,
ULONG key
)
{
if (key == FIFO_KEY_HEAD)
dllInsert (pQFifoHead, (DL_NODE *)NULL, pQFifoNode);
else
dllAdd (pQFifoHead, pQFifoNode);
}
void qPriListPutFromTail
(
Q_PRI_HEAD *pQPriHead,
Q_PRI_NODE *pQPriNode,
ULONG key
)
{
FAST Q_PRI_NODE *pQNode = (Q_PRI_NODE *) DLL_LAST (pQPriHead);
pQPriNode->key = key;
while (pQNode != NULL)
{
if (key >= pQNode->key) /* it will be last of same priority */
{
dllInsert (pQPriHead, &pQNode->node, &pQPriNode->node);
return;
}
pQNode = (Q_PRI_NODE *) DLL_PREVIOUS (&pQNode->node);
}
dllInsert (pQPriHead, (DL_NODE *)NULL, &pQPriNode->node);
}
LOCAL void qFifoPut(
Q_FIFO_HEAD *pQFifoHead,
Q_FIFO_NODE *pQFifoNode,
int key
)
{
if (key == FIFO_KEY_HEAD)
{
dllInsert(
&pQFifoHead->qFifo.head,
(DL_NODE *) NULL,
&pQFifoNode->qFifo.node
);
}
else
{
dllAdd(&pQFifoHead->qFifo.head, &pQFifoNode->qFifo.node);
}
}
static STATUS memPartAddToPool(PART_ID partId, char* pPool, unsigned poolSize)
{
BLOCK_HDR* pHdrStart;
BLOCK_HDR* pHdrMid;
BLOCK_HDR* pHdrEnd;
char* tmp;
int reducePool;
if(!(IS_CLASS(partId, memPartClassId))) /* only memPartClass can call this function */
{
return (ERROR);
}
tmp = (char*) MEM_ROUND_UP(pPool);
reducePool = tmp - pPool;
/* adjust the lenght */
if(poolSize >= reducePool)
{
poolSize -= reducePool;
}
else
{
poolSize = 0;
}
pPool = tmp;
poolSize = MEM_ROUND_DOWN(poolSize);
/* at least one valid free block and three header blocks */
if((sizeof(BLOCK_HDR)*3 + (partId->minBlockWords*2)) > poolSize)
{
return (ERROR);
}
/* initialize three blocks */
pHdrStart = (BLOCK_HDR*)pPool;
pHdrStart->prevHdr = NULL;
pHdrStart->free = FALSE; /* never in use */
pHdrStart->nWords = sizeof(BLOCK_HDR) >> 1;
pHdrMid = NEXT_HDR(pHdrStart);
pHdrMid->prevHdr = pHdrStart;
pHdrMid->free = TRUE; /* the main block */
pHdrMid->nWords = (poolSize - 2*sizeof(BLOCK_HDR)) >> 1;
pHdrEnd = NEXT_HDR(pHdrMid);
pHdrEnd->prevHdr = pHdrMid;
pHdrEnd->free = FALSE;
pHdrEnd->nWords = sizeof (BLOCK_HDR) >> 1;
/* TODO take sem hear */
semTake(partId->semPartId, WAIT_FOREVER);
dllInsert(&partId->freeList, (DL_NODE*)NULL, HDR_TO_NODE(pHdrMid));
partId->totalWords += (poolSize >> 1);
/* TODO give sem hear */
semGive(partId->semPartId);
return (OK);
}
STATUS memPartFree
(
PART_ID partId, /* memory partition to add block to */
char *pBlock /* pointer to block of memory to free */
)
{
FAST BLOCK_HDR *pHdr;
FAST unsigned nWords;
FAST BLOCK_HDR *pNextHdr;
if (ID_IS_SHARED (partId)) /* partition is shared? */
{
if (smMemPartFreeRtn == NULL)
{
errno = S_smObjLib_NOT_INITIALIZED;
return (ERROR);
}
return ((*smMemPartFreeRtn) (SM_OBJ_ID_TO_ADRS (partId), pBlock));
}
/* partition is local */
if (OBJ_VERIFY (partId, memPartClassId) != OK)
return (ERROR);
if (pBlock == NULL)
return (OK); /* ANSI C compatibility */
pHdr = BLOCK_TO_HDR (pBlock);
/* get exclusive access to the partition */
semTake (&partId->sem, WAIT_FOREVER);
/* optional check for validity of block */
if ((partId->options & MEM_BLOCK_CHECK) &&
!memPartBlockIsValid (partId, pHdr, FALSE))
{
semGive (&partId->sem); /* release mutual exclusion */
if (memPartBlockErrorRtn != NULL)
(* memPartBlockErrorRtn) (partId, pBlock, "memPartFree");
if (partId->options & MEM_BLOCK_ERROR_SUSPEND_FLAG)
{
if ((taskIdCurrent->options & VX_UNBREAKABLE) == 0)
taskSuspend (0);
}
errnoSet (S_memLib_BLOCK_ERROR);
return (ERROR);
}
#ifdef WV_INSTRUMENTATION
EVT_OBJ_3 (OBJ, partId, memPartClassId, EVENT_MEMFREE, partId, pBlock, 2 * (pHdr->nWords));
#endif
nWords = pHdr->nWords;
/* check if we can coalesce with previous block;
* if so, then we just extend the previous block,
* otherwise we have to add this as a new free block */
if (PREV_HDR (pHdr)->free)
{
pHdr->free = FALSE; /* this isn't a free block */
pHdr = PREV_HDR (pHdr); /* coalesce with prev block */
pHdr->nWords += nWords;
}
else
{
pHdr->free = TRUE; /* add new free block */
dllInsert (&partId->freeList, (DL_NODE *) NULL, HDR_TO_NODE (pHdr));
}
/* check if we can coalesce with next block;
* if so, then we can extend our block delete next block from free list */
pNextHdr = NEXT_HDR (pHdr);
if (pNextHdr->free)
{
pHdr->nWords += pNextHdr->nWords; /* coalesce with next */
dllRemove (&partId->freeList, HDR_TO_NODE (pNextHdr));
}
/* fix up prev info of whatever block is now next */
NEXT_HDR (pHdr)->pPrevHdr = pHdr;
/* adjust allocation stats */
partId->curBlocksAllocated--;
partId->curWordsAllocated -= nWords;
semGive (&partId->sem);
return (OK);
}
STATUS memPartAddToPool
(
FAST PART_ID partId, /* partition to initialize */
FAST char *pPool, /* pointer to memory block */
FAST unsigned poolSize /* block size in bytes */
)
{
FAST BLOCK_HDR *pHdrStart;
FAST BLOCK_HDR *pHdrMid;
FAST BLOCK_HDR *pHdrEnd;
char * tmp;
int reducePool; /* reduce size of pool by this amount*/
if (ID_IS_SHARED (partId)) /* partition is shared? */
{
if (smMemPartAddToPoolRtn == NULL)
{
errno = S_smObjLib_NOT_INITIALIZED;
return (ERROR);
}
return ((*smMemPartAddToPoolRtn) (SM_OBJ_ID_TO_ADRS (partId),
pPool, poolSize));
}
/* partition is local */
if (OBJ_VERIFY (partId, memPartClassId) != OK)
return (ERROR);
/* insure that the pool starts on an even byte boundary */
tmp = (char *) MEM_ROUND_UP (pPool); /* get actual start */
reducePool = tmp - pPool;
if (poolSize >= reducePool) /* adjust length */
poolSize -= reducePool;
else
poolSize = 0;
pPool = tmp;
/*
* Make sure poolSize is an even multiple of the memory alignment size
* and is large enough to include at least one valid free block and
* three header blocks.
*/
poolSize = MEM_ROUND_DOWN (poolSize);
if (poolSize < ((sizeof (BLOCK_HDR) * 3) + (partId->minBlockWords * 2)))
{
errno = S_memLib_INVALID_NBYTES;
return (ERROR);
}
/* initialize three blocks -
* one at each end of the pool for end cases and real initial free block */
pHdrStart = (BLOCK_HDR *) pPool;
pHdrStart->pPrevHdr = NULL;
pHdrStart->free = FALSE;
pHdrStart->nWords = sizeof (BLOCK_HDR) >> 1;
pHdrMid = NEXT_HDR (pHdrStart);
pHdrMid->pPrevHdr = pHdrStart;
pHdrMid->free = TRUE;
pHdrMid->nWords = (poolSize - 2 * sizeof (BLOCK_HDR)) >> 1;
pHdrEnd = NEXT_HDR (pHdrMid);
pHdrEnd->pPrevHdr = pHdrMid;
pHdrEnd->free = FALSE;
pHdrEnd->nWords = sizeof (BLOCK_HDR) >> 1;
semTake (&partId->sem, WAIT_FOREVER);
#ifdef WV_INSTRUMENTATION
EVT_OBJ_2 (OBJ, partId, memPartClassId, EVENT_MEMADDTOPOOL, partId, poolSize);
#endif
dllInsert (&partId->freeList, (DL_NODE *) NULL, HDR_TO_NODE (pHdrMid));
partId->totalWords += (poolSize >> 1);
semGive (&partId->sem);
return (OK);
}
