/**
 * VxWorks Memory Pool Check                                   V1.00, 14.6.2002
 * -------------------------                           (c) Rene H. Straub, 2002
 * 
 * see notes in header file.
 **/

//--- includes ----------------------------------------------------------------

#include <stdio.h>
#include <string.h>
#include <memory_check_vx.h>



//--- defines ------------------------------------------------------------------

#define MAX_FREE_NODES    (2500)      // Max. number of free memory nodes
#define MAX_ALLOC_BLOCKS  (25000)     // Max. number of allocated blocks per 
                                      // free node.


#ifdef SHOW_LEVEL_ERROLY
# define P_INFO(x)
# define P_MEDIUM(x)  
# define P_ERROR(x)   printf x
#endif

#ifdef SHOW_LEVEL_ERRFRR 
# define P_INFO(x)
# define P_MEDIUM(x)  printf x
# define P_ERROR(x)   printf x
#endif 

#ifdef SHOW_LEVEL_ERRALL
# define P_INFO(x)    printf x
# define P_MEDIUM(x)  printf x
# define P_ERROR(x)   printf x
#endif



//--- local variables ----------------------------------------------------------

static PARTITION*   part        = 0;                  // The partition to check
static void*        memLow      = (void*)0x00000000;  // Lowest valid address 
static void*        memHigh     = (void*)0xffffffff;  // Highest valid address 

static int          errors;                           // Nbr. of errors encountered
static unsigned     totalAlloc;                       // Statistical information
static unsigned     totalFree;
static unsigned     totalAllocNodes;
static unsigned     totalFreeNodes;
static unsigned     largestAlloc;
static unsigned     largestFree;




//--- local functions ----------------------------------------------------------

static void checkPtr(void* ptr, const char* pMsg)
{
  if ((ptr < memLow) || (ptr > memHigh))
  {
    errors++;

    if (pMsg != 0x00000000)
      P_ERROR(("Pointer %8p not in memory region (%8p..%8p)\n", ptr, memLow, memHigh));
    else
      P_ERROR(("%s: Pointer %8p not in memory region (%8p..%8p)\n", pMsg, ptr, memLow, memHigh));
  }
}

//-----------------------------------------------------------------------------

static void existsFreeNode(FREE_BLOCK* pFreeBlock)
{
  // Try to find the specified free block in the list of free nodes.

  DL_NODE*      ndCurrent;
  DL_NODE*      ndRef;
  unsigned      nNodes    = 0;

  checkPtr(part, "Memory Partition");

  ndRef         = &pFreeBlock->node;
  ndCurrent     = DLL_FIRST(&part->freeList);
  checkPtr(ndCurrent, "First node");

  while (ndCurrent != 0x00000000)
  {
    checkPtr(ndCurrent, "existsFreeNode() Free node");

    if (ndCurrent == ndRef)
      return;

    ndCurrent   = DLL_NEXT(ndCurrent);

    if (++nNodes == MAX_FREE_NODES)
    {
      errors++;
      P_ERROR(("Detected more than %d memory nodes.\n", nNodes));
      break;
    }
  };

  P_ERROR(("Free block %p not found in free node list.\n", pFreeBlock));
}

//-----------------------------------------------------------------------------

static void checkMemoryList(FREE_BLOCK* pFreeBlock)
{
  // Check integrity of allocated memory in "managed" by the specified
  // free node.

  BLOCK_HDR*  pMemBlock;
  BLOCK_HDR*  pLastBlock;
  unsigned    nBlock;

  nBlock      = 0;

  pLastBlock  = &pFreeBlock->hdr;
  pMemBlock   = NEXT_HDR(pLastBlock);

  checkPtr(pMemBlock, "Block");

  while (pMemBlock != 0x00000000)
  {
    unsigned    nMemBlockSize;
    int         bMemBlockFree;
 
    checkPtr(pMemBlock, "Block");

    nMemBlockSize = pMemBlock->nWords * 2;
    bMemBlockFree = pMemBlock->free;

    P_INFO((" Block %3d:  Address: %8p  Size: %8d  Next: %p\n", 
      nBlock, pMemBlock, nMemBlockSize, (char*)(pMemBlock) + nMemBlockSize
      ));

    // This nodes previous pointer must point to the last node we processed.
    if (pMemBlock->pPrevHdr != pLastBlock)
    {
      errors++;
      P_ERROR(("mem list broken  current: %p, curr->prev: %p, last: %p\n", 
        pMemBlock, pMemBlock->pPrevHdr, pLastBlock));
      break;
    }

    // The end of the memory list is terminated with a dummy block,
    // just big enough to hold a BLOCK_HDR but no user data. 
    if (nMemBlockSize == sizeof(BLOCK_HDR))
    {
      break;
    }

    // If we encounter a memory block marked as free, then our search ends
    // here. The blocks from here on, will be checked when this free node 
    // is processed.
    if (bMemBlockFree)
    {
      existsFreeNode( (FREE_BLOCK*)pMemBlock ); 
      
      break;
    }

    // Statistics
    totalAllocNodes++;
    totalAlloc  += nMemBlockSize;
    if (nMemBlockSize > largestAlloc)
      largestAlloc = nMemBlockSize;


    // Goto next block
    pLastBlock  = pMemBlock;
    pMemBlock   = NEXT_HDR(pMemBlock);
    
    if (++nBlock == MAX_ALLOC_BLOCKS)
    {
      P_ERROR(("Detected more than %d memory blocks. Aborting.\n", nBlock));
      break;
    }
  }

  P_INFO(("\n"));
}

//-----------------------------------------------------------------------------

static void checkNodeList(DL_NODE* ndFirst, DL_NODE* ndLast)
{
  DL_NODE*      ndPrev    = 0x00000000;
  DL_NODE*      ndCurrent = ndFirst;
  unsigned      nNodes    = 0;

  while (ndCurrent != 0x00000000)
  {
    FREE_BLOCK*     pFreeBlock;
    unsigned        nBlockSize;
    int             bBlockUsed;

    checkPtr(ndCurrent, "Node");

    // Display information
    pFreeBlock  = (FREE_BLOCK*) NODE_TO_HDR(ndCurrent);   // Pointer to free block header
    nBlockSize  = pFreeBlock->hdr.nWords * 2;             // Size in bytes
    if (pFreeBlock->hdr.free)
      bBlockUsed = false;
    else
      bBlockUsed = true;

    P_INFO(("Node %3d: Address: %8p  Size: %8d\n", nNodes, pFreeBlock, nBlockSize));

    // Validate node
    if (pFreeBlock->node.previous != ndPrev)
    {
      errors++;
      P_MEDIUM((" Node %p: Previous pointer %p invalid (should be %p)\n", 
        pFreeBlock, pFreeBlock->node.previous, ndPrev));
    }

    if (bBlockUsed)
    {
      errors++;
      P_ERROR((" Node %p: Marked as used.\n", pFreeBlock));
    }

    // Statistics
    totalFreeNodes++;
    totalFree += nBlockSize;
    if (nBlockSize > largestFree)
      largestFree = nBlockSize;


    /*
     * Go through all memory blocks that are linked from here.
     */
    checkMemoryList(pFreeBlock);


    // Goto next node in free list.
    ndPrev      = ndCurrent;
    ndCurrent   = DLL_NEXT(&pFreeBlock->node);

    if (++nNodes == MAX_FREE_NODES)
    {
      errors++;
      P_ERROR(("Detected more than %d memory nodes.\n", nNodes));
      break;
    }
  };

  // Check list header last pointer versus last free node element found.
  if (ndPrev != ndLast)
  {
    errors++;
    P_ERROR(("List header last pointer %p does not point to last node %p\n", ndLast, ndPrev));
  }
}



//--- global functions ---------------------------------------------------------

void memCheckSetup(PART_ID partition, void* lowAddr, void* highAddr)
{
  if (partition == 0)
  {
    // Use VxWorks default memory partition
    part = memSysPartId;
  }
  else
  {
    // Use callers memory partition
    part = partition; 
  }

  // Remember this memory partitions address range, so we can check pointers.
  memLow    = lowAddr;
  memHigh   = highAddr;
}

//-----------------------------------------------------------------------------

void memCheck(const char* pszFile, unsigned line)
{
  DL_NODE*      ndFirst;
  DL_NODE*      ndLast;

  if (part == 0) 
  {
    P_ERROR(("Memory check not initialized. Call memCheckSetup() first\n."));
  }

  checkPtr(part, "Memory Partition");

  // Take mem list semaphore. This will block everyone trying to allocate/free
  // memory...
  semTake(&part->sem, WAIT_FOREVER);      

  // Init variables to scan free memory list.
  errors            = 0;
  totalAlloc        = 0;
  totalFree         = 0;
  totalAllocNodes   = 0;
  totalFreeNodes    = 0;
  largestAlloc      = 0;
  largestFree       = 0;
  
  ndFirst     = DLL_FIRST(&part->freeList);
  ndLast      = DLL_LAST (&part->freeList);
  checkPtr(ndFirst, "First node");
  checkPtr(ndLast, "Last node");

  /*
   * Scan through list of free memory
   */
  checkNodeList(ndFirst, ndLast);


  // Show collected statistics
  P_MEDIUM(("\n             Alloc      Free     Total\n"
            "-------- --------- --------- -----------\n"));
  P_MEDIUM((" Blocks   %8d  %8d  %8d\n", totalAllocNodes, totalFreeNodes, totalAllocNodes+totalFreeNodes));
  P_MEDIUM((" Bytes    %8d  %8d  %8d\n", totalAlloc, totalFree, totalAlloc+totalFree));
  P_MEDIUM((" Largest  %8d  %8d\n\n", largestAlloc, largestFree));


  // Release semaphore
  (void)semGive(&part->sem);


  // If errors occurred display message and halt system. 
  if (errors != 0)
  {
    P_ERROR(("Memory check %s (%d) failed with %d error(s). Halting task.\n", 
      pszFile, line, errors));

    taskSuspend(0);
  }
}

//-----------------------------------------------------------------------------

void memCheckTest(void)
{
  char*   p1;
  char*   p2;
  
  // Setup test. Don't know valid address range so use max. possible range.
  memCheckSetup(0, (void*)0x00000000, (void*)0xffffffff);
  
  // Alloc some memory we can use for test
  p1 = (char*)malloc(16);
  printf("Allocated 16 bytes at %p\n", p1);
  p2 = (char*)malloc(16);
  printf("Allocated 16 bytes at %p\n", p2);

  // No error until now
  MEM_CHECK();

  // Write beyond the memory allocated for <p2>
  // On a PPC memory is aligned on 8 byte boundaries. Thus we now overwrite
  // the memory node of <p1>.
  memset(p2+16, 0x33, 4);
  printf("Writing 4 bytes beyond allocated memory at %p\n", p2);

  // This test must fail
  MEM_CHECK();

}