memp.nc

IEEE802.15.4标准下的基于ZIGBEE协议栈的物理及链路层的代码

/*
 *
 * the malloced memory head and data 
 * -----------------------------------------------------------------------------
 * |                 hdr             |         data(size of byte )              |
 * -----------------------------------------------------------------------------
 * | 76543210 |      7    |  6543210 |                    Date begin at right-> |
 * -----------------------------------------------------------------------------
 * |   size   |  free bit |    pos   |<-this is the pos which Mem.malloc return |
 * -----------------------------------------------------------------------------
 *
 *
 * the free memory head and data in heap
 * -----------------------------------------------------------------------------
 * |                 hdr                    |         data(size of byte )       |
 * -----------------------------------------------------------------------------
 * |   6543210   |    15     |    14-8      |                                   |
 * -----------------------------------------------------------------------------
 * |size low byte| free bit  |size high byte|                                   |
 * -----------------------------------------------------------------------------
 *
 * The memory heap  
 * --------------------------------------------------------------
 * |  hdr  |  data    |  hdr  |  data   |...|  hdr  |  data    |
 * --------------------------------------------------------------
 * | 2byte |n byte    | 2byte | n byte  |...| 2byte |n byte    |
 * --------------------------------------------------------------                               
 *
 * mem_head_t define: typedef uint8_t mem_head_t;
 * 
 * because storaging data from right to left, so we can caculate length of data by "size - pos"  
 *
 *
 * note that when malloc a block which size is size_t and it is malloced from a block which size is  (size_t+1),
 * the returned size is (size_t+1) other than size_t, because a free block must at least 2 byte(the length of header).  
 *
 *
 */

module MemP
{
	provides interface Mem;
	provides interface Init;
}

implementation
{


#define HEAP_SIZE 1024
#define MALLOC_MIN_SIZE 4
#define MALLOC_MAX_SIZE 127
uint8_t heap[HEAP_SIZE];

void setFree(uint8_t* hdr);
void setUse(uint8_t* hdr);

uint8_t getFree(uint8_t* hdr);

void setSize(uint8_t* hdr, uint16_t size);

uint16_t getSize(uint8_t* hdr);



/***************************************************************************/

void setFree(uint8_t* hdr)
{	
	*(hdr+1) &= 0x7F;
}


void setUse(uint8_t* hdr)
{
	*(hdr+1) |= 0x80;
}


/* free:0 | use:1 */
uint8_t getFree(uint8_t* hdr)
{
	return *(hdr+1)&(0x80);
}


void setSize(uint8_t* hdr, uint16_t size)
{
	*(hdr+1) = size>>8;
	*hdr = size&0xFF;
}


uint16_t getSize(uint8_t* hdr)
{
	uint16_t size = 0;
	if( getFree(hdr) == 0x80 ){	//have malloced to use
		size = (*hdr)&0x7F;
	}
	else{					//do not malloced
		size = *(hdr+1);
		size <<= 8;
		size += (*hdr)&0xFF;
	}
	return size;
}


command error_t Init.init()
{
	setFree(heap);
	setSize(heap, HEAP_SIZE-2);
//	dbg("TestMem", "initing\n\n\n");
	return SUCCESS;
}


async command error_t Mem.malloc(mem_head_t** p, uint8_t size)
{
	uint16_t offset = 0;
	uint8_t* phdr = heap;
	uint8_t found = 0;
	if(size<MALLOC_MIN_SIZE){
		dbg("TestMem", "malloc size must be greater than MALLOC_MIN_SIZE\n");
		return FAIL;
	}
	if(size>MALLOC_MAX_SIZE){
		dbg("TestMem", "malloc size must be small than MALLOC_MAX_SIZE\n");
		return FAIL;
	}

	atomic{
	while(1){
		if(getFree(phdr)==0 && getSize(phdr)>=size){
			found = 1;
			break;
		}
		else{
			offset += getSize(phdr)+2;
			if(offset >= HEAP_SIZE-MALLOC_MIN_SIZE-2)
				break;
			phdr = heap+offset;
		}
	}
	if(found == 0){
		dbg("TestMem", "Can't find a free block whose size is greater than %d\n", size);
		return FAIL;
	}
//	dbg("TestMem", "find a new block which size is %d\n", getSize(phdr) );
//	dbg("TestMem", "this block size byte is %d\t:%d\n", *phdr, *(phdr+1) );
	if( getSize(phdr) >= (size+2) ){
		setFree(phdr+size+2);
		setSize((phdr+size+2), getSize(phdr)-size-2);
		setSize(phdr, size);
	}
	setUse(phdr);
	//add 0x80 in order to set free bit
	*(phdr+1) = getSize(phdr) - size + 0x80;            //set current position
	*p = (mem_head_t*)(phdr+2);
	return SUCCESS;
	}
	
}


async command error_t Mem.free(mem_head_t* p)
{
	uint8_t* phdr = heap;
	uint8_t* ptmp = heap; 
	uint16_t offset = 0;
	if(!p)
		return FAIL;
	if ( ( *(p-1) & (0x80) ) == 0 )
		return SUCCESS;
	atomic{
//	setFree( (uint8_t*)p -2 );
	*( (uint8_t*)p -1) = 0;            //reset pos and clear free bit
	while(1){
		if(getFree(phdr) == 0){
			if(offset+getSize(phdr)+2 >= HEAP_SIZE)       //is the last head
				break;
			ptmp = phdr+getSize(phdr)+2;
			if(getFree(ptmp) == 0){
				setSize(phdr, getSize(phdr)+getSize(ptmp)+2);
				continue;
			}
			offset += getSize(phdr)+getSize(ptmp)+4;       //skip the two heads
			if(offset >= HEAP_SIZE)
				break; 	
			phdr = heap+offset;
		}
		else{
			offset += getSize(phdr)+2;
			if(offset >= HEAP_SIZE)
				break;
			phdr = heap+offset;
		}
	}
	}

	return SUCCESS;
}	


async command uint8_t Mem.getSize(mem_head_t* p)
{
	atomic return (*(uint8_t*)(p-2))&0x7F;
}


async command uint8_t Mem.getCurrentPos(mem_head_t* p)
{
	atomic return *(uint8_t*)(p-1) & (0x7F);
}


async command error_t Mem.setCurrentPos(mem_head_t* p, uint8_t pos)
{
	atomic *(uint8_t*)(p-1) = (pos&0x7F) + 0x80;
	return SUCCESS;
}


async command void Mem.dumpMem()
{
	uint8_t nums = 0;
	uint8_t* phdr = heap;
	uint16_t offset = 0;
	atomic
	{
	while(1){
		if(offset < HEAP_SIZE){
			dbg("TestMem", "this is the %dth block at offset %d\n", nums++, offset);		
			dbg("TestMem", "block head is %d:%d\tsize is %d\n\n", *phdr, *(phdr+1), getSize(phdr) );
			offset += getSize(phdr) + 2;
			phdr = heap + offset;
		}
		else
			break;
	}
	}

}


}