mempool.c

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

#endif
	};
	MPLCB	*mplcb;
	ID	mplid;
	INT	mplsz;
	VP	mempool;

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

	/* Cannot use non-resident memory if TA_TPRI is used, */
	if ( (pk_cmpl->mplatr & (TA_TPRI|TA_NORESIDENT))
				== (TA_TPRI|TA_NORESIDENT) ) return E_NOSPT;

	mplsz = ROUND(pk_cmpl->mplsz);

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

	/* Get control block from FreeQue */
	DISABLE_INTERRUPT;
	mplcb = (MPLCB*)QueRemoveNext(&free_mplcb);
	ENABLE_INTERRUPT;
	if ( mplcb == NULL ) {
		IAfree(mempool, pk_cmpl->mplatr);
		return E_LIMIT;
	}

	LockOBJ(&mplcb->lock);
	mplid = ID_MPL(mplcb - mplcb_table);

	/* Initialize control block */
	QueInit(&mplcb->wait_queue);
	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(mplcb->name, pk_cmpl->dsname, OBJECT_NAME_LENGTH);
	}
#endif

	/* Initialize memory pool */
	init_mempool(mplcb, mempool, mplsz + sizeof(QUEUE)*2);

	mplcb->mplid  = mplid;  /* Set ID at last*/
	UnlockOBJ(&mplcb->lock);

	return mplid;
}

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

	CHECK_MPLID(mplid);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);

	LockOBJ(&mplcb->lock);
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		DISABLE_INTERRUPT;
		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;
		ENABLE_INTERRUPT;
	}
	UnlockOBJ(&mplcb->lock);

	if ( mempool != NULL ) 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 = ROUND(blksz);

	LockOBJ(&mplcb->lock);
	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;
	}

	/* Get memory block */
	blk = get_blk(mplcb, blksz);
	if ( blk == NULL ) goto wait_mpl; /* Go to get wait if cannot get */

	*p_blk = blk;

    error_exit:
	UnlockOBJ(&mplcb->lock);

	return ercd;

    wait_mpl:
	/* Ready for wait */
	BEGIN_CRITICAL_SECTION;
	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);

	UnlockOBJ(&mplcb->lock);
	END_CRITICAL_SECTION;

	return ercd;
}

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

	CHECK_MPLID(mplid);
	CHECK_DISPATCH();

	mplcb = get_mplcb(mplid);

	LockOBJ(&mplcb->lock);
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
		goto error_exit;
	}
#ifdef 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 */
	DISABLE_INTERRUPT;
	while ( !isQueEmpty(&mplcb->wait_queue) ) {
		tcb = (TCB*)mplcb->wait_queue.next;
		wspec = tcb->wspec;
		blksz = tcb->winfo.mpl.blksz;

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

		/* Get memory block */
		blk = get_blk(mplcb, blksz);
		DISABLE_INTERRUPT;

		if ( (tcb->state & TS_WAIT) == 0
		   || tcb->wspec != wspec
		   || tcb->wid != mplcb->mplid ) {
			/* Wait was freed while getting memory block */
			ENABLE_INTERRUPT;
			rel_blk(mplcb, blk);
			DISABLE_INTERRUPT;
			continue;
		}
		*tcb->winfo.mpl.p_blk = blk;

		/* Wake get wait task */
		wait_release_ok(tcb);
	}
	ENABLE_INTERRUPT;

    error_exit:
	UnlockOBJ(&mplcb->lock);

	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);

	LockOBJ(&mplcb->lock);
	if ( mplcb->mplid == 0 ) {
		ercd = E_NOEXS;
	} else {
		DISABLE_INTERRUPT;
		pk_rmpl->wtsk = wait_tskid(&mplcb->wait_queue);
		ENABLE_INTERRUPT;
		pk_rmpl->exinf = mplcb->exinf;
		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);
	}
	UnlockOBJ(&mplcb->lock);

	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 if ( isLockedOBJ(&mplcb->lock) ) {
		ercd = E_CTX;
	} else {
		pk_rmpl->wtsk  = wait_tskid(&mplcb->wait_queue);
		pk_rmpl->exinf = mplcb->exinf;
		if ( (mplcb->mplatr & TA_NORESIDENT) == 0 ) {
			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);
		} else {
			/* Unable to access if it is non-resident memory.
			   So, it is unknown. */
			pk_rmpl->frsz  = -1;
			pk_rmpl->maxsz = -1;
		}
	}
	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 */