#include "mem_mod.h"

/* FreeRTOS includes */
#include "FreeRTOS.h"
#include "Task.h"
#include "Queue.h"
     
extern void serWrite (
  int device, 
  short length, /**< Lengh of data in bytes */ 
  char * data   /**< Pointer to data */
);

void Memmod_Init(memman *me,unsigned char buf_count,unsigned short buf_size)
{
    unsigned char *buffer;
    unsigned short i;
    me->count = buf_count;
    me->size = buf_size;
    buffer = pvPortMalloc(buf_count*buf_size);
    me->buffer = buffer;
    me->free_index = buf_count;
    me->freelist = pvPortMalloc(buf_count*sizeof(link_item));
    for(i=0;i<buf_count;i++)
    {
        me->freelist[i].data = buffer;
        buffer = buffer+buf_size;
    }
}

unsigned char* Memmod_GetBuffer(memman *me)
{
	return me->buffer;
}

memman *Memmod_Create(unsigned char buf_count,unsigned short buf_size)
{
    memman *new_item;
    new_item = (memman *)pvPortMalloc(sizeof(memman));
    Memmod_Init(new_item,buf_count,buf_size);
    return new_item;
}

void *Memmod_Alloc(memman *me)
{
    unsigned char index;
    void *retval;

    taskENTER_CRITICAL();
    {
        index = me->free_index;
        if(index > 0)
        {
            index--;
            me->free_index=index;
            retval = me->freelist[index].data;
        }
        else
        {
            retval = 0;
        }
    }
    taskEXIT_CRITICAL();
    
	if (retval == 0)
	{
	    serWrite(1, sizeof("buffer error"), "buffer error");
	}

    return retval;
}

void Memmod_Free(memman *me,void *buffer)
{
    unsigned char index;

    taskENTER_CRITICAL();
    {
        index = me->free_index;
        if(index < me->count)
        {
            me->freelist[index].data = buffer;
            index++;
            me->free_index=index;
        }
    }
    taskEXIT_CRITICAL();

}