xiaqian369

xiaqian369

关注
发信
关注(28)粉丝(399)

vxworks下的malloc实现函数


点击(此处)折叠或打开

  1. /*! Janson(http://www.gandancing.com) 2011-2012
  2.  *
  3.  *\file
  4.  *\brief
  5.  *\details
  6.  *
  7.  *\author Janson
  8.  *\version
  9.  *\date 04Jan12
  10.  *
  11.  *\warning
  12.  *
  13.  *\history \arg 30Jan12, Janson, Create the file
  14.  * modify from VxWorks source
  15.  * Legal Declaration: it is for studying VxWorks only.
  16.  */
  17. #include "includes.h"
  18.  
  19. /* The memParLib.h file is hear:
  20. https://code.google.com/p/vxworks-like-kernel/source/browse/trunk/inc/private/memPartLib.h
  21. */
  22.  
  23. /* optional check for bad blocks */
  24.  
  25. #define MEM_BLOCK_CHECK 0x10
  26.  
  27. /* response to errors when allocating memory */
  28.  
  29. #define MEM_ALLOC_ERROR_LOG_FLAG 0x20
  30. #define MEM_ALLOC_ERROR_SUSPEND_FLAG 0x40
  31.  
  32. /* response to errors when freeing memory */
  33.  
  34. #define MEM_BLOCK_ERROR_LOG_FLAG 0x80
  35. #define MEM_BLOCK_ERROR_SUSPEND_FLAG 0x100
  36.  
  37. #define NEXT_HDR(pHdr) ((BLOCK_HDR *) ((char *) (pHdr) + (2 * (pHdr)->nWords)))
  38. #define PREV_HDR(pHdr) ((pHdr)->prevHdr)
  39.  
  40. #define HDR_TO_BLOCK(pHdr) ((char *) ((int) pHdr + sizeof (BLOCK_HDR)))
  41. #define BLOCK_TO_HDR(pBlock) ((BLOCK_HDR *) ((int) pBlock - \
  42.                         sizeof(BLOCK_HDR)))
  43.  
  44. #define HDR_TO_NODE(pHdr) (& ((FREE_BLOCK *) pHdr)->node)
  45. #define NODE_TO_HDR(pNode) ((BLOCK_HDR *) ((int) pNode - \
  46.                         OFFSET (FREE_BLOCK, node)))
  47.  
  48. static BOOL memPartLibInstalled = FALSE;
  49.  
  50. static OBJ_CLASS memPartClass;
  51. CLASS_ID memPartClassId = &memPartClass;
  52.  
  53. static PARTITION memSysPartition;
  54. PART_ID memSysPartId = &memSysPartition;
  55. U32 memDefaultAlign = _ALLOC_ALIGN_SIZE;
  56.  
  57. static SEMAPHORE semMemSysPartition;
  58.  
  59. static void memPartSemInit(PART_ID partId);
  60.  
  61. FUNCTION memPartSemInitRtn = (FUNCTION) memPartSemInit;
  62.  
  63. unsigned memPartDefaultOption = MEM_BLOCK_ERROR_SUSPEND_FLAG | MEM_BLOCK_CHECK;
  64.  
  65. static BOOL memPartBlockIsValid(PART_ID partId, FAST BLOCK_HDR* pHdr, BOOL isFree);
  66. static BLOCK_HDR* memAlignedBlockSplit(PART_ID partId
  67.     , FAST BLOCK_HDR* pHdr
  68.     , FAST unsigned nWords
  69.     , unsigned minWords
  70.     , unsigned align);
  71.  
  72. static STATUS memPartAddToPool(PART_ID partId, char* pPool, unsigned poolSize);
  73.  
  74. STATUS memPartLibInit(char* pPool, unsigned poolSize)
  75. {
  76.     if((!memPartLibInstalled) &&
  77.         (OK == classInit(memPartClassId, sizeof(PARTITION), OFFSET(PARTITION, objCore)
  78.         , (FUNCTION)memPartCreate, (FUNCTION)memPartInit, (FUNCTION)memPartDestroy)))
  79.     {
  80.         memPartInit(&memSysPartition, pPool, poolSize);
  81.         memPartLibInstalled = TRUE;
  82.     }
  83.  
  84.     return ((memPartLibInstalled)? OK : ERROR);
  85. }
  86.  
  87. PART_ID memPartCreate(char* pPool, unsigned poolSize)
  88. {
  89.     PART_ID pPart = (PART_ID)objAlloc(memPartClassId);
  90.  
  91.     if(NULL != pPart)
  92.     {
  93.         memPartInit(pPart, pPool, poolSize);
  94.     }
  95.  
  96.     return pPart;
  97. }
  98.  
  99. void memPartInit(PART_ID partId, char* pPool, unsigned poolSize)
  100. {
  101.     memset((void*)partId, 0, sizeof(*partId));
  102.  
  103.     partId->options = memPartDefaultOption;
  104.     partId->minBlockWords = sizeof (FREE_BLOCK) >> 1; /* word not byte */
  105.  
  106.     (* memPartSemInitRtn) (partId);
  107.      
  108.     dllInit(&partId->freeList);
  109.      
  110.     objCoreInit(&partId->objCore, memPartClassId);
  111.      
  112.     memPartAddToPool(partId, pPool, poolSize);
  113. }
  114.  
  115. STATUS memPartDestroy(PART_ID partId)
  116. {
  117.     return (ERROR);
  118. }
  119.  
  120. void memAddToPool(FAST char *pPool, FAST unsigned poolSize)
  121. {
  122.     (void)memPartAddToPool(&memSysPartition, pPool, poolSize);
  123. }
  124.  
  125.  
  126. static STATUS memPartAddToPool(PART_ID partId, char* pPool, unsigned poolSize)
  127. {
  128.     BLOCK_HDR* pHdrStart;
  129.     BLOCK_HDR* pHdrMid;
  130.     BLOCK_HDR* pHdrEnd;
  131.     char* tmp;
  132.     int reducePool;
  133.  
  134.     if(!(IS_CLASS(partId, memPartClassId))) /* only memPartClass can call this function */
  135.     {
  136.         return (ERROR);
  137.     }
  138.  
  139.     tmp = (char*) MEM_ROUND_UP(pPool);
  140.     reducePool = tmp - pPool;
  141.  
  142.     /* adjust the lenght */
  143.     if(poolSize >= reducePool)
  144.     {
  145.         poolSize -= reducePool;
  146.     }
  147.     else
  148.     {
  149.         poolSize = 0;
  150.     }
  151.     pPool = tmp;
  152.      
  153.     poolSize = MEM_ROUND_DOWN(poolSize);
  154.      
  155.     /* at least one valid free block and three header blocks */
  156.     if((sizeof(BLOCK_HDR)*3 + (partId->minBlockWords*2)) > poolSize)
  157.     {
  158.         return (ERROR);
  159.     }
  160.  
  161.     /* initialize three blocks */
  162.     pHdrStart = (BLOCK_HDR*)pPool;
  163.     pHdrStart->prevHdr = NULL;
  164.     pHdrStart->free = FALSE; /* never in use */
  165.     pHdrStart->nWords = sizeof(BLOCK_HDR) >> 1;
  166.  
  167.     pHdrMid = NEXT_HDR(pHdrStart);
  168.     pHdrMid->prevHdr = pHdrStart;
  169.     pHdrMid->free = TRUE; /* the main block */
  170.     pHdrMid->nWords = (poolSize - 2*sizeof(BLOCK_HDR)) >> 1;
  171.  
  172.     pHdrEnd = NEXT_HDR(pHdrMid);
  173.     pHdrEnd->prevHdr = pHdrMid;
  174.     pHdrEnd->free = FALSE;
  175.     pHdrEnd->nWords = sizeof (BLOCK_HDR) >> 1;
  176.  
  177.     /* TODO take sem hear */
  178.     semTake(partId->semPartId, WAIT_FOREVER);
  179.      
  180.     dllInsert(&partId->freeList, (DL_NODE*)NULL, HDR_TO_NODE(pHdrMid));
  181.     partId->totalWords += (poolSize >> 1);
  182.  
  183.     /* TODO give sem hear */
  184.     semGive(partId->semPartId);
  185.  
  186.  
  187.     return (OK);
  188. }
  189.  
  190. void* memPartAllignedAlloc(FAST PART_ID partId, unsigned nBytes, unsigned align)
  191. {
  192.     FAST unsigned nWords;
  193.     FAST unsigned nWordsExtra;
  194.     FAST DL_NODE* pNode;
  195.     FAST BLOCK_HDR* pHdr;
  196.     BLOCK_HDR* pNewHdr;
  197.     BLOCK_HDR* origpHdr;
  198.  
  199.     if(!(IS_CLASS(partId, memPartClassId))) /* only memPartClass can call this function */
  200.     {
  201.         return (NULL);
  202.     }
  203.  
  204.     nWords = (MEM_ROUND_UP(nBytes)+sizeof(BLOCK_HDR)) >>1;
  205.  
  206.     if((nWords<<1) < nBytes)
  207.     {
  208.         /* TODO suspend the task */
  209.         return (NULL);
  210.     }
  211.  
  212.     if(nWords < partId->minBlockWords)
  213.     {
  214.         nWords = partId->minBlockWords;
  215.     }
  216.  
  217.     /* TODO task the semaphore hear */
  218.     semTake(partId->semPartId, WAIT_FOREVER);
  219.     pNode = DLL_FIRST(&partId->freeList);
  220.     nWordsExtra = nWords + align/2; /* why? */
  221.  
  222.     for(;;)
  223.     {
  224.         while(NULL != pNode)
  225.         {
  226.             if((NODE_TO_HDR(pNode)->nWords > nWordsExtra) ||
  227.                 ((NODE_TO_HDR(pNode)->nWords == nWords) &&
  228.                 (ALIGNED(HDR_TO_BLOCK(NODE_TO_HDR(pNode)), align))))
  229.             {
  230.                 break;
  231.             }
  232.  
  233.             pNode = DLL_NEXT(pNode);
  234.         }
  235.  
  236.         if(NULL == pNode)
  237.         {
  238.             /*TODO give the semaphore */
  239.             semGive(partId->semPartId);
  240.             return NULL;
  241.         }
  242.  
  243.         pHdr = NODE_TO_HDR(pNode);
  244.         origpHdr = pHdr;
  245.  
  246.         pNewHdr = memAlignedBlockSplit(partId, pHdr, nWords, partId->minBlockWords, align);
  247.         if(NULL != pNewHdr)
  248.         {
  249.             pHdr = pNewHdr;
  250.             break;
  251.         }
  252.  
  253.         pNode = DLL_NEXT(pNode);
  254.     }
  255.  
  256.     pHdr->free = FALSE;
  257.     partId->allBlocksAlloc++;
  258.     partId->allWordsAlloc += pHdr->nWords;
  259.     partId->curBlocksAlloc++;
  260.     partId->curWordsAlloc += pHdr->nWords;
  261.  
  262.     /*TODO give the semaphore hear */
  263.     semGive(partId->semPartId);
  264.     return (HDR_TO_BLOCK(pHdr));
  265.      
  266. }
  267.  
  268. void* memPartAlloc(FAST PART_ID partId, unsigned bytes)
  269. {
  270.     return memPartAllignedAlloc(partId, bytes, memDefaultAlign);
  271. }
  272.  
  273. STATUS memPartFree(PART_ID partId, char* pBlock)
  274. {
  275.     FAST BLOCK_HDR *pHdr;
  276.     FAST unsigned nWords;
  277.     FAST BLOCK_HDR *pNextHdr;
  278.  
  279.     if(!IS_CLASS(partId, memPartClassId))
  280.     {
  281.         return (ERROR);
  282.     }
  283.  
  284.     if(NULL == pBlock)
  285.     {
  286.         return (OK);
  287.     }
  288.  
  289.     pHdr = BLOCK_TO_HDR(pBlock);
  290.  
  291.     semTake(partId->semPartId, WAIT_FOREVER);
  292.  
  293.     if((partId->options & MEM_BLOCK_CHECK)
  294.         && !memPartBlockIsValid(partId, pHdr, FALSE))
  295.     {
  296.         semGive(partId->semPartId);
  297.         return (ERROR);
  298.     }
  299.  
  300.     nWords = pHdr->nWords;
  301.     if(PREV_HDR(pHdr)->free)
  302.     {/* the prev hdr is free and than coalesce with it */
  303.         pHdr->free = FALSE;
  304.         pHdr = PREV_HDR(pHdr);
  305.         pHdr->nWords += nWords;
  306.     }
  307.     else
  308.     {
  309.         pHdr->free = TRUE;
  310.         dllInsert(&partId->freeList, (DL_NODE*)NULL, HDR_TO_NODE(pHdr));
  311.     }
  312.  
  313.     /* check to coalesce with the next */
  314.     pNextHdr = NEXT_HDR(pHdr);
  315.     if(pNextHdr->free)
  316.     {
  317.         pHdr->nWords += pNextHdr->nWords;
  318.         dllRemove(&partId->freeList, HDR_TO_NODE(pNextHdr));
  319.     }
  320.  
  321.     /* cannot use pNextHdr->prevHdr=pHdr hear */
  322.     NEXT_HDR(pHdr)->prevHdr = pHdr;
  323.  
  324.     partId->curBlocksAlloc--;
  325.     partId->curWordsAlloc -= nWords;
  326.  
  327.     /* TODO give sem hear */
  328.     semGive(partId->semPartId);
  329.      
  330.     return (OK);
  331. }
  332.  
  333. static BOOL memPartBlockIsValid(PART_ID partId, FAST BLOCK_HDR* pHdr, BOOL isFree)
  334. {
  335.     BOOL valid;
  336.  
  337.     TASK_LOCK();
  338.     semGive(partId->semPartId);
  339.      
  340.     valid = MEM_ALIGNED(pHdr)
  341.     && MEM_ALIGNED(pHdr->nWords*2)
  342.     && (pHdr->nWords < partId->totalWords)
  343.     && (pHdr->free == isFree)
  344.     && (pHdr == PREV_HDR(NEXT_HDR(pHdr)))
  345.     && (pHdr == NEXT_HDR(PREV_HDR(pHdr)));
  346.      
  347.     semTake(partId->semPartId, WAIT_FOREVER);
  348.     TASK_UNLOCK();
  349.  
  350.     return valid;
  351. }
  352.  
  353. static BLOCK_HDR* memAlignedBlockSplit(PART_ID partId
  354.     , FAST BLOCK_HDR* pHdr
  355.     , FAST unsigned nWords
  356.     , unsigned minWords
  357.     , unsigned align)
  358. {
  359.     FAST BLOCK_HDR *pNewHdr;
  360.     FAST BLOCK_HDR *pNextHdr;
  361.     FAST char *endOfBlock;
  362.     FAST char *pNewBlock;
  363.     int blockSize;
  364.  
  365.     endOfBlock = (char*)pHdr + (pHdr->nWords*2);
  366.  
  367.     pNewBlock = (char*)((unsigned)endOfBlock - ((nWords - sizeof(BLOCK_HDR)/2)*2));
  368.  
  369.     pNewBlock = (char*)((unsigned)pNewBlock & (~(align-1)));
  370.  
  371.     pNewHdr = BLOCK_TO_HDR(pNewBlock);
  372.  
  373.     blockSize = ((char*)pNewHdr - (char*)pHdr)/2;
  374.  
  375.     if(blockSize < minWords)
  376.     {
  377.         if(pNewHdr == pHdr)
  378.         {
  379.             dllRemove(&partId->freeList, HDR_TO_NODE(pHdr));
  380.         }
  381.         else
  382.         {
  383.             return NULL;
  384.         }
  385.     }
  386.     else
  387.     { /* recaculate pHdr */
  388.         pNewHdr->prevHdr = pHdr;
  389.         pHdr->nWords = blockSize;
  390.     }
  391.  
  392.     if(((U32)endOfBlock - (U32)pNewHdr - (nWords*2)) < (minWords*2))
  393.     {
  394.         pNewHdr->nWords = (endOfBlock - pNewBlock + sizeof(BLOCK_HDR))/2;
  395.         pNewHdr->free = TRUE;
  396.  
  397.         NEXT_HDR(pNewHdr)->prevHdr = pNewHdr;
  398.     }
  399.     else
  400.     {/* space left is enough to be a fragment on the free list then */
  401.         pNewHdr->nWords = nWords;
  402.         pNewHdr->free = TRUE;
  403.  
  404.         pNextHdr = NEXT_HDR(pNewHdr);
  405.         /* words 包括BlockHdr */
  406.         pNextHdr->nWords = ((U32)endOfBlock - (U32)pNextHdr) / 2;
  407.         pNextHdr->prevHdr = pNewHdr;
  408.         pNextHdr->free = TRUE;
  409.  
  410.         dllAdd(&partId->freeList, HDR_TO_NODE(pNextHdr));
  411.  
  412.         NEXT_HDR(pNextHdr)->prevHdr = pNewHdr;
  413.     }
  414.  
  415.     return (pNewHdr);
  416. }
  417.  
  418. static void memPartSemInit(PART_ID partId)
  419. {
  420.     semBInit(&semMemSysPartition, SEM_Q_FIFO, SEM_FULL);
  421.  
  422.     partId->semPartId = &semMemSysPartition;
  423. }
  424.  
  425. void* malloc(unsigned bytes)
  426. {
  427.     return memPartAlloc(memSysPartId, bytes);
  428. }
  429.  
  430. void free(void* p)
  431. {
  432.     memPartFree(memSysPartId, (char*)p);
  433. }

09-26 19:34
Powered by LMLPHP ©2024 xiaqian369 0.080650