mempool.c

日本著名的的嵌入式实时操作系统T-Kernel的源码及用户手册。

/*
 *----------------------------------------------------------------------
 *    T-Kernel
 *
 *    Copyright (C) 2004 by Ken Sakamura. All rights reserved.
 *    T-Kernel is distributed under the T-License.
 *----------------------------------------------------------------------
 *
 *    Version:   1.01.00
 *    Released by T-Engine Forum(http://www.t-engine.org) at 2004/6/28.
 *
 *----------------------------------------------------------------------
 */

/*
 *	mempool.c (T-Kernel/OS)
 *	Variable Size Memory Pool
 */

#include "kernel.h"
#include "task.h"
#include "wait.h"
#include "check.h"

#ifdef NUM_MPLID

EXPORT ID	max_mplid;	/* Maximum variable size memory pool ID */

/*
 * Variable size memory pool control block
 *	'areaque' connects memory blocks in address ascending order
 *	'freeque' connects memory blocks in size increasing order
 */
typedef struct memorypool_control_block {
	QUEUE	wait_queue;	/* Memory pool wait queue*/
	ID	mplid;		/* Variable size memory pool ID */
	VP	exinf;		/* Extended information */
	ATR	mplatr;		/* Memory pool attribute */
	INT	mplsz;		/* Whole memory pool size */
	QUEUE	areaque;	/* Queue connecting all blocks */
	QUEUE	freeque;	/* Queue connecting free blocks */
	OBJLOCK	lock;		/* Lock for object exclusive access */
#if USE_OBJECT_NAME
	UB	name[OBJECT_NAME_LENGTH];	/* name */
#endif
} MPLCB;

LOCAL MPLCB	*mplcb_table;	/* Variable size memory pool control block */
LOCAL QUEUE	free_mplcb;	/* FreeQue */

#define get_mplcb(id)	( &mplcb_table[INDEX_MPL(id)] )


/*
 * Initialization of variable size memory pool control block
 */
EXPORT ER memorypool_initialize( void )
{
	MPLCB	*mplcb, *end;
	W	n;

	/* Get system information */
	n = _tk_get_cfn("TMaxMplId", &max_mplid, 1);
	if ( n < 1 || NUM_MPLID < 1 ) return E_SYS;

	/* Create variable size memory pool control block */
	mplcb_table = Imalloc(NUM_MPLID * sizeof(MPLCB));
	if ( mplcb_table == NULL ) return E_NOMEM;

	/* Register all control blocks onto FeeQue*/
	QueInit(&free_mplcb);
	end = mplcb_table + NUM_MPLID;
	for ( mplcb = mplcb_table; mplcb < end; mplcb++ ) {
		mplcb->mplid = 0;
		InitOBJLOCK(&mplcb->lock);
		QueInsert(&mplcb->wait_queue, &free_mplcb);
	}

	return E_OK;
}

/* ------------------------------------------------------------------------ */

/*
 * Minimum unit of subdivision
 *	The lower 3 bits of the address is always 0
 *	because memory is allocated by ROUNDSZ.
 *	AreaQue uses the lowest bits for flag.
 */
#define	ROUNDSZ		( sizeof(QUEUE) )	/* 8 bytes */

/*
 * Minimum fragment size (the smallest size to store FreeQue)
 */
#define	MIN_FRAGMENT	( sizeof(QUEUE) * 2 )

/*
 * Adjusting the size which can be allocated 
 */
Inline INT ROUND( INT sz )
{
	if ( sz < MIN_FRAGMENT ) sz = MIN_FRAGMENT;
	return (sz + (ROUNDSZ-1)) & ~(ROUNDSZ-1);
}

/*
 * Flag that uses the lower bits of AreaQue's 'prev'.
 */
#define	AREA_USE	0x00000001	/*In-use */
#define	AREA_MASK	0x00000001

#define	setAreaFlag(q, f)   ( (q)->prev = (QUEUE*)((UINT)(q)->prev |  (f)) )
#define	clrAreaFlag(q, f)   ( (q)->prev = (QUEUE*)((UINT)(q)->prev & ~(f)) )
#define	chkAreaFlag(q, f)   ( ((UINT)(q)->prev & (f)) != 0 )

#define	Mask(x)		( (QUEUE*)((UINT)(x) & ~AREA_MASK) )
#define	Assign(x, y)	( (x) = (QUEUE*)((UINT)(x) & AREA_MASK | (UINT)(y)) )

/*
 * Area size
 */
#define	AreaSize(aq)	( (VB*)(aq)->next - (VB*)((aq) + 1) )
#define	FreeSize(fq)	( (VB*)((fq) - 1)->next - (VB*)(fq) )

/*
 * Maximum free area size
 */
Inline INT MaxFreeSize( MPLCB *mplcb )
{
	if ( isQueEmpty(&mplcb->freeque) ) return 0;
	return FreeSize(mplcb->freeque.prev);
}

/*
 * FreeQue search
 *	Search the free area whose size is equal to 'blksz',
 *	or larger than
 *      'blksz' but closest.
 *	If it does not exist, return '&mplcb->freeque'.
 */
LOCAL QUEUE* searchFreeArea( MPLCB *mplcb, INT blksz )
{
	QUEUE	*q = &mplcb->freeque;

	/* For area whose memory pool size is less than 1/4,
	   search from smaller size.
	   Otherwise, search from larger size. */
	if ( blksz > mplcb->mplsz / 4 ) {
		/* Search from larger size. */
		INT fsz = 0;
		while ( (q = q->prev) != &mplcb->freeque ) {
			fsz = FreeSize(q);
			if ( fsz <= blksz ) return ( fsz < blksz )? q->next: q;
		}
		return ( fsz >= blksz )? q->next: q;
	} else {
		/* Search from smaller size. */
		while ( (q = q->next) != &mplcb->freeque ) {
			if ( FreeSize(q) >= blksz ) break;
		}
		return q;
	}
}

/*
 * Registration of free area on FreeQue
 *	FreeQue is composed of 2 types: Queue that links the
 *	different size of areas by size and queue that links the
 *	same size of areas.
 *
 *	freeque
 *	|
 *	|   +-----------------------+	    +-----------------------+
 *	|   | AreaQue		    |	    | AreaQue		    |
 *	|   +-----------------------+	    +-----------------------+
 *	*---> FreeQue Size order    |  *----> FreeQue Same size   ----->
 *	|   | FreeQue Same size   -----*    | EmptyQue		    |
 *	|   |			    |	    |			    |
 *	|   |			    |	    |			    |
 *	|   +-----------------------+	    +-----------------------+
 *	|   | AreaQue		    |	    | AreaQue		    |
 *	v   +-----------------------+	    +-----------------------+
 */
LOCAL void appendFreeArea( MPLCB *mplcb, QUEUE *aq )
{
	QUEUE	*fq;
	INT	size = AreaSize(aq);

	/* Registration position search */
	/*  Search the free area whose size is equal to 'blksz',
	 *  or larger than 'blksz' but closest.
	 *  If it does not exist, return '&mplcb->freeque'.
	 */
	fq = searchFreeArea(mplcb, size);

	/* Register */
	clrAreaFlag(aq, AREA_USE);
	if ( fq != &mplcb->freeque && FreeSize(fq) == size ) {
		QueInsert(aq + 1, fq + 1);
	} else {
		QueInsert(aq + 1, fq);
	}
	QueInit(aq + 2);
}

/*
 * Delete from FreeQue
 */
LOCAL void removeFreeQue( QUEUE *fq )
{
	if ( !isQueEmpty(fq + 1) ) {
		QUEUE *nq = (fq + 1)->next;

		QueRemove(fq + 1);
		QueInsert(nq + 1, nq);
		QueRemove(nq);
		QueInsert(nq, fq);
	}

	QueRemove(fq);
}

/*
 * Register area
 *	Insert 'ent' just after 'que.'
 */
LOCAL void insertAreaQue( QUEUE *que, QUEUE *ent )
{
	ent->prev = que;
	ent->next = que->next;
	Assign(que->next->prev, ent);
	que->next = ent;
}

/*
 * Delete area
 */
LOCAL void removeAreaQue( QUEUE *aq )
{
	Mask(aq->prev)->next = aq->next;
	Assign(aq->next->prev, Mask(aq->prev));
}

/*
 * Get memory block 
 *	'blksz' must be larger than minimum fragment size
 *	and adjusted by ROUNDSZ unit.
 */
LOCAL VP get_blk( MPLCB *mplcb, INT blksz )
{
	QUEUE	*q, *aq;

	/* Search FreeQue */
	q = searchFreeArea(mplcb, blksz);
	if ( q == &mplcb->freeque ) return NULL;

	/* remove free area from FreeQue */
	removeFreeQue(q);
	aq = q - 1;

	/* If there is a fragment smaller than the minimum fragment size,
	   allocate them together */
	if ( AreaSize(aq) - blksz >= MIN_FRAGMENT + sizeof(QUEUE) ) {

		/* Divide the area into 2. */
		q = (QUEUE*)((VB*)(aq + 1) + blksz);
		insertAreaQue(aq, q);

		/* Register the remaining area onto FreeQue */
		appendFreeArea(mplcb, q);
	}
	setAreaFlag(aq, AREA_USE);

	return (VP)(aq + 1);
}

/*
 * Free memory block 
 */
LOCAL ER rel_blk( MPLCB *mplcb, VP blk )
{
	QUEUE	*aq;

	aq = (QUEUE*)blk - 1;

#ifdef CHK_PAR
	if ( !chkAreaFlag(aq, AREA_USE) ) return E_PAR;
#endif
	clrAreaFlag(aq, AREA_USE);

	if ( !chkAreaFlag(aq->next, AREA_USE) ) {
		/* Merge to the next area */
		removeFreeQue(aq->next + 1);
		removeAreaQue(aq->next);
	}
	if ( !chkAreaFlag(aq->prev, AREA_USE) ) {
		/* Merge to the previous area */
		aq = aq->prev;
		removeFreeQue(aq + 1);
		removeAreaQue(aq->next);
	}

	/* Register free area onto FreeQue */
	appendFreeArea(mplcb, aq);

	return E_OK;
}

/*
 * Memory pool initial setting
 */
LOCAL void init_mempool( MPLCB *mplcb, VP mempool, INT mempsz )
{
	QUEUE	*tp, *ep;

	QueInit(&mplcb->areaque);
	QueInit(&mplcb->freeque);

	/* Register onto AreaQue */
	tp = (QUEUE*)mempool;
	ep = (QUEUE*)((VB*)mempool + mempsz) - 1;
	insertAreaQue(&mplcb->areaque, ep);
	insertAreaQue(&mplcb->areaque, tp);

	/* Set AREA_USE for locations that must not be free area */
	setAreaFlag(&mplcb->areaque, AREA_USE);
	setAreaFlag(ep, AREA_USE);

	/* Register onto FreeQue */
	appendFreeArea(mplcb, tp);
}

/* ------------------------------------------------------------------------ */

/*
 * Processing if the priority of wait task changes
 *	Since you need to execute with interrupt disable,
 *	you cannot use it for the non-resident memory.
 */
LOCAL void mpl_chg_pri( TCB *tcb, INT oldpri )
{
	MPLCB	*mplcb;
	TCB	*top;
	VP	blk;
	INT	blksz;

	mplcb = get_mplcb(tcb->wid);
	if ( oldpri >= 0 ) {
		/* Reorder wait line */
		gcb_change_priority((GCB*)mplcb, tcb);
	}

	/* From the new top task of a wait queue, free the assign
	   wait of memory blocks as much as possible.*/
	while ( !isQueEmpty(&mplcb->wait_queue) ) {
		top = (TCB*)mplcb->wait_queue.next;
		blksz = top->winfo.mpl.blksz;

		/* Check free space */
		if ( blksz > MaxFreeSize(mplcb) ) break;

		/* Get memory block */
		blk = get_blk(mplcb, blksz);
		*top->winfo.mpl.p_blk = blk;

		/* Wake wait task */
		wait_release_ok(top);
	}
}

/*
 * Processing if the wait task is freed
 */
LOCAL void mpl_rel_wai( TCB *tcb )
{
	mpl_chg_pri(tcb, -1);
}

/*
 * Definition of variable size memory pool wait specification
 */
LOCAL WSPEC wspec_mpl_tfifo = { TTW_MPL, NULL, NULL };
LOCAL WSPEC wspec_mpl_tpri  = { TTW_MPL, mpl_chg_pri, mpl_rel_wai };


/*
 * Create variable size memory pool 
 */
SYSCALL ID _tk_cre_mpl( T_CMPL *pk_cmpl )
{
	const ATR VALID_MPLATR = {
		 TA_TPRI
		|TA_RNG3
		|TA_NORESIDENT
		|TA_NODISWAI
#if USE_OBJECT_NAME
		|TA_DSNAME