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); }