/* debug variants of memory allocation routines 
   to log memory requests
   
   Set break point in panic() to halt program when it feels something is not right. 
   The call Stack can then be traversed to determine the cause of the problem.
   
   Change the dbg_size value (in an expressions' window) and set breakpoint on 
   for loop in StopOnSize() to halt the program when a request to allocate or 
   release a particular size of memory is made. Usual for tracking down memory
   leaks.
   
   current_mem_allocated shows the total memory currently allocated via these 
   functions; so if _adi_fss_malloc() is always used in all parts of the FSS 
   FSDs & PIDs then this tells us how much heap the File system stuff is consuming 
   at any time.
   
   peak_mem_allocated gives the high tide mark for overall memory usage.
   
   Both of these can be viewed in an expressions' window, but there are also
   a couple of functions that can be used in an application to interrogate them. Just 
   place the following statements in your code to use.
   
   extern u32 MemUsage(void);
   extern u32 PeakMemUsage(void);

   The fss_pmp_demo537 project calls these via the commands:
   
   mem
   mem peak
   
   To use this module in the above project, enable the _DEBUG_ALLOC_ macro in the 
   File options for adi_fss_data.c
   
*/

#define _DBG_MEM_HEAP_ID_ 1

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <services/services.h>
#include <heapinfo.h>

typedef struct {
	u32 addr;
	u32 alloc;
	size_t size;
} ALLOC_ENTRY;
static ALLOC_ENTRY *pAllocTab;
static u32 count=0;
static u32 tableSize = 0;
static u32 current_mem_allocated[MAXHEAPS] = {0, 0, 0, 0};
static u32 peak_mem_allocated[MAXHEAPS] = {0, 0, 0, 0};

#if 1
static char * HeapNames[MAXHEAPS] = {
	"FAT Housekeeping" ,
	"FSS Heap        ",
	"FAT Data Buffers",
	"L1  Heap        ",
};
#else
static char * HeapNames[MAXHEAPS] = {
	"System  Heap",
	"General Heap",
	"Cache   Heap",
	"L1      Heap",
};
#endif

static void panic(void)
{
	printf("\n*** PANIC *** \n");
	while(1);
}

static size_t dbg_size = 0x7FFFFFFF;

static void StopOnSize(size_t size)
{
	u32 i;
	if (size==dbg_size)
	    for (i=0;i<1;i++);
}

static void AdjustMemoryCounters(int id, size_t size, u32 add)
{
	if (add) {
    	current_mem_allocated[id] += size;
    	if (current_mem_allocated[id] > peak_mem_allocated[id])
    	    peak_mem_allocated[id] = current_mem_allocated[id];
	} else {
        if (current_mem_allocated[id] < size)
            panic();
        current_mem_allocated[id] -= size;
	}
}

static ALLOC_ENTRY *FindEntry( u32 addr )
{
	u32 i;
	ALLOC_ENTRY *pe = &pAllocTab[0];
	for (i=0;i<count && pe->addr!=addr;i++,pe++);
	if (i!=count)
	    return pe;
	
    return NULL;
}
	   
static void AssignEntry( int id, u32 p, size_t size )
{
    ALLOC_ENTRY *pe = FindEntry ( (u32)p );
    if (pe && pe->alloc==1) 
        panic();
    else if (pe) {
    	pe->size = size;
    	AdjustMemoryCounters(id, size, 1);
        pe->alloc = 1;
    } else {
        if (count)
            pAllocTab = (ALLOC_ENTRY*)heap_realloc(_DBG_MEM_HEAP_ID_,pAllocTab,(count+1)*sizeof(ALLOC_ENTRY));
        else
            pAllocTab = (ALLOC_ENTRY*)heap_malloc(_DBG_MEM_HEAP_ID_,sizeof(ALLOC_ENTRY));
    
        tableSize = (count+1)*sizeof(ALLOC_ENTRY);
                  
        pAllocTab[count].addr = (u32)p;
        pAllocTab[count].alloc = 1;
        pAllocTab[count].size = size;
    	AdjustMemoryCounters(id,size, 1);
        count++;
        if (count==0xFFFFFFFF)
            panic();
    }
}


void *_adi_dbg_malloc( int id, size_t size )
{
	u32 i;
    void *p = heap_malloc(id,size);
    StopOnSize(size);
    if (!p) panic();
    AssignEntry(id, (u32)p, size);
        
    //printf("MALLOC %08X (%d)\n", p, size);
    return p;
}

void *_adi_dbg_realloc( int id, void *p, size_t size )
{
    ALLOC_ENTRY *pe = FindEntry ( (u32)p );
    if (!pe) 
        panic();
    else {
        pe->alloc = 0;
    	AdjustMemoryCounters(id, pe->size, 0);
        pe->size = 0;
    }
    void *p2 = heap_realloc(id,p,size);
    
    AssignEntry(id, (u32)p2, size);
            
    //printf("REALLOC %08X -> %08X (%d)\n", p1, p2, size);
    return p2;
}

void _adi_dbg_free( int id, void *p )
{
    //printf("FREE %08X\n", p);
    ALLOC_ENTRY *pe = FindEntry ( (u32)p );
    if (!pe || pe->alloc==0)
        panic();
    else {
        StopOnSize(pe->size);
        heap_free(id,p);
        pe->alloc = 0;
    	AdjustMemoryCounters(id, pe->size, 0);
        pe->size = 0;
    }  
    return;
}

void MemUsage(void)
{
	int i;
    printf("%11s\t%8s\t%8s\n","Type       ","Used","Peak");
    //printf("Type          Used\t\tPeak\n");
    for (i=0;i<MAXHEAPS;i++)
	{
    	int heapIndex = heap_lookup(i);
		if (heapIndex!=-1)
		{
        	printf("%11s\t%8d\t%8d\n",HeapNames[i],current_mem_allocated[i],peak_mem_allocated[i]);
			
		}
	}
}

void ResetMemUsageCounters(void)
{
	int i;
    for (i=0;i<MAXHEAPS;i++)
	{
    	int heapIndex = heap_lookup(i);
		if (heapIndex!=-1)
		{
        	current_mem_allocated[i] = 0;
            peak_mem_allocated[i] = 0;
		}
	}
	MemUsage();
}