mempool.c

使用广泛的日本著名的开源嵌入式实时操作系统T-Kernel的源码

	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. */
	mpl_wakeup(mplcb);
}

/*
 * 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,        mpl_rel_wai };
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_NODISWAI
#if USE_OBJECT_NAME
		|TA_DSNAME
#endif
	};
	MPLCB	*mplcb;
	ID	mplid;
	INT	mplsz;
	VP	mempool;
	ER	ercd;

	CHECK_RSATR(pk_cmpl->mplatr, VALID_MPLATR);
	CHECK_PAR(pk_cmpl->mplsz > 0);
	CHECK_DISPATCH();

	mplsz = roundSize(pk_cmpl->mplsz);

	/* Allocate memory for memory pool */
	mempool = IAmalloc((UINT)mplsz + sizeof(QUEUE)*2, pk_cmpl->mplatr);
	if ( mempool == NULL ) {
		return E_NOMEM;
	}

	BEGIN_CRITICAL_SECTION;
	/* Get control block from FreeQue */
	mplcb = (MPLCB*)QueRemoveNext(&free_mplcb);
	if ( mplcb == NULL ) {
		ercd = E_LIMIT;
	} else {
		mplid = ID_MPL(mplcb - mplcb_table);

		/* Initialize control block */
		QueInit(&mplcb->wait_queue);
		mplcb->mplid  = mplid;
		mplcb->exinf  = pk_cmpl->exinf;
		mplcb->mplatr = pk_cmpl->mplatr;
		mplcb->mplsz  = mplsz;
#if USE_OBJECT_NAME
		if ( (pk_cmpl->mplatr & TA_DSNAME) != 0 ) {
			strncpy((char*)mplcb->name, (char*)pk_cmpl->dsname, OBJECT_NAME_LENGTH);
		}
#endif
		/* Initialize memory pool */
		init_mempool(mplcb, mempool, mplsz + (INT)sizeof(QUEUE)*2);

		ercd = mplid;
	}
	END_CRITICAL_SECTION;

	if ( ercd < E_OK ) {
		IAfree(mempool, pk_cmpl->mplatr);
	}

	return ercd;
}

/*
 * Delete variable size memory pool 
 */
SYSCALL ER _tk_del_mpl( ID mplid )
{
	MPLCB	*mplcb;
	VP	mempool = NULL;
	ATR	memattr = 0;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);

	BEGIN_CRITICAL_SECTION;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		mempool = mplcb->areaque.next;
		memattr = mplcb->mplatr;

		/* Free wait state of task (E_DLT) */
		wait_delete(&mplcb->wait_queue);

		/* Return to FreeQue */
		QueInsert(&mplcb->wait_queue, &free_mplcb);
		mplcb->mplid = 0;
	}
	END_CRITICAL_SECTION;

	if ( ercd == E_OK ) {
		IAfree(mempool, memattr);
	}

	return ercd;
}

/*
 * Get variable size memory block 
 */
SYSCALL ER _tk_get_mpl( ID mplid, INT blksz, VP* p_blk, TMO tmout )
{
	MPLCB	*mplcb;
	VP	blk = NULL;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);
	CHECK_PAR(blksz > 0);
	CHECK_TMOUT(tmout);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);
	blksz = roundSize(blksz);

	BEGIN_CRITICAL_SECTION;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
		goto error_exit;
	}

	/* Check wait disable */
	if ( is_diswai((GCB*)mplcb, ctxtsk, TTW_MPL) ) {
		ercd = E_DISWAI;
		goto error_exit;
	}

	if ( gcb_top_of_wait_queue((GCB*)mplcb, ctxtsk) == ctxtsk
	  && (blk = get_blk(mplcb, blksz)) != NULL ) {
		/* Get memory block */
		*p_blk = blk;
	} else {
		/* Ready for wait */
		ctxtsk->wspec = ( (mplcb->mplatr & TA_TPRI) != 0 )?
					&wspec_mpl_tpri: &wspec_mpl_tfifo;
		ctxtsk->wercd = &ercd;
		ctxtsk->winfo.mpl.blksz = blksz;
		ctxtsk->winfo.mpl.p_blk = p_blk;
		gcb_make_wait_with_diswai((GCB*)mplcb, tmout);
	}

    error_exit:
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Return variable size memory block 
 */
SYSCALL ER _tk_rel_mpl( ID mplid, VP blk )
{
	MPLCB	*mplcb;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);

	BEGIN_CRITICAL_SECTION;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
		goto error_exit;
	}
#if CHK_PAR
	if ( (B*)blk < (B*)mplcb->areaque.next || (B*)blk > (B*)mplcb->areaque.prev ) {
		ercd = E_PAR;
		goto error_exit;
	}
#endif

	/* Free memory block */
	ercd = rel_blk(mplcb, blk);
	if ( ercd < E_OK ) {
		goto error_exit;
	}

	/* Assign memory block to waiting task */
	mpl_wakeup(mplcb);

    error_exit:
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Refer variable size memory pool state
 */
SYSCALL ER _tk_ref_mpl( ID mplid, T_RMPL *pk_rmpl )
{
	MPLCB	*mplcb;
	QUEUE	*fq, *q;
	INT	frsz, blksz;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);

	BEGIN_CRITICAL_SECTION;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		pk_rmpl->exinf = mplcb->exinf;
		pk_rmpl->wtsk  = wait_tskid(&mplcb->wait_queue);
		frsz = 0;
		for ( fq = mplcb->freeque.next; fq != &mplcb->freeque; fq = fq->next ) {
			blksz = FreeSize(fq);
			frsz += blksz;
			for ( q = (fq+1)->next; q != (fq+1); q = q->next ) {
				frsz += blksz;
			}
		}
		pk_rmpl->frsz  = frsz;
		pk_rmpl->maxsz = MaxFreeSize(mplcb);
	}
	END_CRITICAL_SECTION;

	return ercd;
}

/* ------------------------------------------------------------------------ */
/*
 *	Debugger support function
 */
#if USE_DBGSPT

/*
 * Get object name from control block
 */
#if USE_OBJECT_NAME
EXPORT ER memorypool_getname(ID id, UB **name)
{
	MPLCB	*mplcb;
	ER	ercd = E_OK;

	CHECK_MPLID(id);

	BEGIN_DISABLE_INTERRUPT;
	mplcb = get_mplcb(id);
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
		goto error_exit;
	}
	if ( (mplcb->mplatr & TA_DSNAME) == 0 ) {
		ercd = E_OBJ;
		goto error_exit;
	}
	*name = mplcb->name;

    error_exit:
	END_DISABLE_INTERRUPT;

	return ercd;
}
#endif /* USE_OBJECT_NAME */

/*
 * Refer variable size memory pool usage state
 */
SYSCALL INT _td_lst_mpl( ID list[], INT nent )
{
	MPLCB	*mplcb, *end;
	INT	n = 0;

	BEGIN_DISABLE_INTERRUPT;
	end = mplcb_table + NUM_MPLID;
	for ( mplcb = mplcb_table; mplcb < end; mplcb++ ) {
		if ( mplcb->mplid == 0 ) {
			continue;
		}

		if ( n++ < nent ) {
			*list++ = ID_MPL(mplcb - mplcb_table);
		}
	}
	END_DISABLE_INTERRUPT;

	return n;
}

/*
 * Refer variable size memory pool state
 */
SYSCALL ER _td_ref_mpl( ID mplid, TD_RMPL *pk_rmpl )
{
	MPLCB	*mplcb;
	QUEUE	*fq, *q;
	INT	frsz, blksz;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);

	mplcb = get_mplcb(mplid);

	BEGIN_DISABLE_INTERRUPT;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		pk_rmpl->exinf = mplcb->exinf;
		pk_rmpl->wtsk  = wait_tskid(&mplcb->wait_queue);
		frsz = 0;
		for ( fq = mplcb->freeque.next; fq != &mplcb->freeque; fq = fq->next ) {
			blksz = FreeSize(fq);
			frsz += blksz;
			for ( q = (fq+1)->next; q != (fq+1); q = q->next ) {
				frsz += blksz;
			}
		}
		pk_rmpl->frsz  = frsz;
		pk_rmpl->maxsz = MaxFreeSize(mplcb);
	}
	END_DISABLE_INTERRUPT;

	return ercd;
}

/*
 * Refer variable size memory pool wait queue 
 */
SYSCALL INT _td_mpl_que( ID mplid, ID list[], INT nent )
{
	MPLCB	*mplcb;
	QUEUE	*q;
	ER	ercd = E_OK;

	CHECK_MPLID(mplid);

	mplcb = get_mplcb(mplid);

	BEGIN_DISABLE_INTERRUPT;
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		INT n = 0;
		for ( q = mplcb->wait_queue.next; q != &mplcb->wait_queue; q = q->next ) {
			if ( n++ < nent ) {
				*list++ = ((TCB*)q)->tskid;
			}
		}
		ercd = n;
	}
	END_DISABLE_INTERRUPT;

	return ercd;
}

#endif /* USE_DBGSPT */
#endif /* NUM_MPLID */