dataqueue.c

嵌入式系统开发 TOPPERS and JSP Kernel Release 1.3 TOPPERS = Toyohashi Open Platform for Embedded Real-Tim

/*
 *  TOPPERS/JSP Kernel
 *      Toyohashi Open Platform for Embedded Real-Time Systems/
 *      Just Standard Profile Kernel
 * 
 *  Copyright (C) 2000,2001 by Embedded and Real-Time Systems Laboratory
 *                              Toyohashi Univ. of Technology, JAPAN
 * 
 *  惧淡螟侯涪荚は·Free Software Foundation によって给山されている 
 *  GNU General Public License の Version 2 に淡揭されている掘凤か·笆
 *  布の(1)×(4)の掘凤を塔たす眷圭に嘎り·塑ソフトウェア∈塑ソフトウェ
 *  アを猖恃したものを崔むˉ笆布票じ∷を蝗脱ˇ剩澜ˇ猖恃ˇ浩芹邵∈笆布·
 *  网脱と钙ぶ∷することを痰浸で钓满するˉ
 *  (1) 塑ソフトウェアをソ〖スコ〖ドの妨で网脱する眷圭には·惧淡の螟侯
 *      涪山绩·この网脱掘凤および布淡の痰瘦沮惮年が·そのままの妨でソ〖
 *      スコ〖ド面に崔まれていることˉ
 *  (2) 塑ソフトウェアを浩网脱材墙なバイナリコ〖ド∈リロケ〖タブルオブ
 *      ジェクトファイルやライブラリなど∷の妨で网脱する眷圭には·网脱
 *      に燃うドキュメント∈网脱荚マニュアルなど∷に·惧淡の螟侯涪山绩·
 *      この网脱掘凤および布淡の痰瘦沮惮年を非很することˉ
 *  (3) 塑ソフトウェアを浩网脱稍材墙なバイナリコ〖ドの妨または怠达に寥
 *      み哈んだ妨で网脱する眷圭には·肌のいずれかの掘凤を塔たすことˉ
 *    (a) 网脱に燃うドキュメント∈网脱荚マニュアルなど∷に·惧淡の螟侯
 *        涪山绩·この网脱掘凤および布淡の痰瘦沮惮年を非很することˉ
 *    (b) 网脱の妨轮を·侍に年める数恕によって·惧淡螟侯涪荚に鼠桂する
 *        ことˉ
 *  (4) 塑ソフトウェアの网脱により木儡弄または粗儡弄に栏じるいかなる禄
 *      巢からも·惧淡螟侯涪荚を倘勒することˉ
 * 
 *  塑ソフトウェアは·痰瘦沮で捏丁されているものであるˉ惧淡螟侯涪荚は·
 *  塑ソフトウェアに簇して·その努脱材墙拉も崔めて·いかなる瘦沮も乖わ
 *  ないˉまた·塑ソフトウェアの网脱により木儡弄または粗儡弄に栏じたい
 *  かなる禄巢に簇しても·その勒扦を砷わないˉ
 * 
 *  @(#) $Id: dataqueue.c,v 1.6 2002/03/26 08:19:38 hiro Exp $
 */

/*
 *	デ〖タキュ〖怠墙
 */

#include "jsp_kernel.h"
#include "check.h"
#include "task.h"
#include "wait.h"
#include "dataqueue.h"

/*
 *  デ〖タキュ〖IDの呵络猛∈kernel_cfg.c∷
 */
extern const ID	tmax_dtqid;

/*
 *  デ〖タキュ〖介袋步ブロックのエリア∈kernel_cfg.c∷
 */
extern const DTQINIB	dtqinib_table[];

/*
 *  デ〖タキュ〖瓷妄ブロックのエリア∈kernel_cfg.c∷
 */
extern DTQCB	dtqcb_table[];

/*
 *  デ〖タキュ〖IDからデ〖タキュ〖瓷妄ブロックを艰り叫すためのマクロ
 */
#define INDEX_DTQ(dtqid)	((dtqid) - TMIN_DTQID)
#define get_dtqcb(dtqid)	(&(dtqcb_table[INDEX_DTQ(dtqid)]))

/*
 *  デ〖タキュ〖略ち攫鼠ブロックの年盗
 *
 *  デ〖タキュ〖への流慨略ちとデ〖タキュ〖からの减慨略ちで·票じ略ち攫
 *  鼠ブロックを蝗うˉ
 */
typedef struct dataqueue_waiting_information {
	WINFO	winfo;		/* 筛洁の略ち攫鼠ブロック */
	WOBJCB	*wobjcb;	/* 略ちオブジェクトのコントロ〖ルブロック */
	VP_INT	data;		/* 流减慨デ〖タ  */
} WINFO_DTQ;

/*
 *  デ〖タキュ〖怠墙の介袋步
 */
void
dataqueue_initialize(void)
{
	INT	i;
	DTQCB	*dtqcb;

	for (dtqcb = dtqcb_table, i = 0; i < tmax_dtqid; dtqcb++, i++) {
		queue_initialize(&(dtqcb->swait_queue));
		dtqcb->dtqinib = &(dtqinib_table[i]);
		queue_initialize(&(dtqcb->rwait_queue));
		dtqcb->count = 0;
		dtqcb->head = 0;
		dtqcb->tail = 0;
	}
}

/*
 *  デ〖タキュ〖挝拌にデ〖タを呈羌
 */
static BOOL
enqueue_data(DTQCB *dtqcb, VP_INT data)
{
	if (dtqcb->count < dtqcb->dtqinib->dtqcnt) {
		*((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->tail) = data;
		dtqcb->count++;
		dtqcb->tail++;
		if (dtqcb->tail >= dtqcb->dtqinib->dtqcnt) {
			dtqcb->tail = 0;
		}
		return(TRUE);
	}
	return(FALSE);
}

/*
 *  デ〖タキュ〖挝拌にデ〖タを动扩呈羌
 */
static void
force_enqueue_data(DTQCB *dtqcb, VP_INT data)
{
	*((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->tail) = data;
	dtqcb->tail++;
	if (dtqcb->tail >= dtqcb->dtqinib->dtqcnt) {
		dtqcb->tail = 0;
	}
	if (dtqcb->count < dtqcb->dtqinib->dtqcnt) {
		dtqcb->count++;
	}
	else {
		dtqcb->head = dtqcb->tail;
	}
}

/*
 *  デ〖タキュ〖挝拌からデ〖タを艰叫し
 */
static BOOL
dequeue_data(DTQCB *dtqcb, VP_INT *p_data)
{
	if (dtqcb->count > 0) {
		*p_data = *((VP_INT *)(dtqcb->dtqinib->dtq) + dtqcb->head);
		dtqcb->count--;
		dtqcb->head++;
		if (dtqcb->head >= dtqcb->dtqinib->dtqcnt) {
			dtqcb->head = 0;
		}
		return(TRUE);
	}
	return(FALSE);
}

/*
 *  减慨略ちキュ〖の黎片タスクへのデ〖タ流慨
 */
static TCB *
send_data_rwait(DTQCB *dtqcb, VP_INT data)
{
	TCB	*tcb;

	if (!(queue_empty(&(dtqcb->rwait_queue)))) {
		tcb = (TCB *) queue_delete_next(&(dtqcb->rwait_queue));
		((WINFO_DTQ *)(tcb->winfo))->data = data;
		return(tcb);
	}
	return(NULL);
}

/*
 *  流慨略ちキュ〖の黎片タスクからのデ〖タ减慨
 */
static TCB *
receive_data_swait(DTQCB *dtqcb, VP_INT *p_data)
{
	TCB	*tcb;

	if (!(queue_empty(&(dtqcb->swait_queue)))) {
		tcb = (TCB *) queue_delete_next(&(dtqcb->swait_queue));
		*p_data = ((WINFO_DTQ *)(tcb->winfo))->data;
		return(tcb);
	}
	return(NULL);
}

/*
 *  デ〖タキュ〖への流慨
 */
SYSCALL ER
snd_dtq(ID dtqid, VP_INT data)
{
	DTQCB	*dtqcb;
	WINFO_DTQ winfo;
	TCB	*tcb;
	ER	ercd;

	CHECK_DISPATCH();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else if (enqueue_data(dtqcb, data)) {
		ercd = E_OK;
	}
	else {
		winfo.data = data;
		wobj_make_wait((WOBJCB *) dtqcb, (WINFO_WOBJ *) &winfo);
		dispatch();
		ercd = winfo.winfo.wercd;
	}
	t_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖への流慨∈ポ〖リング∷
 */
SYSCALL ER
psnd_dtq(ID dtqid, VP_INT data)
{
	DTQCB	*dtqcb;
	TCB	*tcb;
	ER	ercd;

	CHECK_TSKCTX_UNL();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else if (enqueue_data(dtqcb, data)) {
		ercd = E_OK;
	}
	else {
		ercd = E_TMOUT;
	}
	t_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖への流慨∈ポ〖リング·润タスクコンテキスト脱∷
 */
SYSCALL ER
ipsnd_dtq(ID dtqid, VP_INT data)
{
	DTQCB	*dtqcb;
	TCB	*tcb;
	ER	ercd;

	CHECK_INTCTX_UNL();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);

	i_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			reqflg = TRUE;
		}
		ercd = E_OK;
	}
	else if (enqueue_data(dtqcb, data)) {
		ercd = E_OK;
	}
	else {
		ercd = E_TMOUT;
	}
	i_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖への流慨∈タイムアウトあり∷
 */
SYSCALL ER
tsnd_dtq(ID dtqid, VP_INT data, TMO tmout)
{
	DTQCB	*dtqcb;
	WINFO_DTQ winfo;
	TMEVTB	tmevtb;
	TCB	*tcb;
	ER	ercd;

	CHECK_DISPATCH();
	CHECK_DTQID(dtqid);
	CHECK_TMOUT(tmout);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else if (enqueue_data(dtqcb, data)) {
		ercd = E_OK;
	}
	else if (tmout == TMO_POL) {
		ercd = E_TMOUT;
	}
	else {
		winfo.data = data;
		wobj_make_wait_tmout((WOBJCB *) dtqcb, (WINFO_WOBJ *) &winfo,
						&tmevtb, tmout);
		dispatch();
		ercd = winfo.winfo.wercd;
	}
	t_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖への动扩流慨
 */
SYSCALL ER
fsnd_dtq(ID dtqid, VP_INT data)
{
	DTQCB	*dtqcb;	
	TCB	*tcb;

	CHECK_TSKCTX_UNL();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);
	CHECK_ILUSE(dtqcb->dtqinib->dtqcnt > 0);

	t_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
	}
	else {
		force_enqueue_data(dtqcb, data);
	}
	t_unlock_cpu();
	return(E_OK);
}

/*
 *  デ〖タキュ〖への动扩流慨∈润タスクコンテキスト脱∷
 */
SYSCALL ER
ifsnd_dtq(ID dtqid, VP_INT data)
{
	DTQCB	*dtqcb;
	TCB	*tcb;

	CHECK_INTCTX_UNL();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);
	CHECK_ILUSE(dtqcb->dtqinib->dtqcnt > 0);

	i_lock_cpu();
	if ((tcb = send_data_rwait(dtqcb, data)) != NULL) {
		if (wait_complete(tcb)) {
			reqflg = TRUE;
		}
	}
	else {
		force_enqueue_data(dtqcb, data);
	}
	i_unlock_cpu();
	return(E_OK);
}

/*
 *  デ〖タキュ〖からの减慨
 */
SYSCALL ER
rcv_dtq(ID dtqid, VP_INT *p_data)
{
	DTQCB	*dtqcb;
	WINFO_DTQ winfo;
	TCB	*tcb;
	VP_INT	data;
	ER	ercd;

	CHECK_DISPATCH();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if (dequeue_data(dtqcb, p_data)) {
		if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
			enqueue_data(dtqcb, data);
			if (wait_complete(tcb)) {
				dispatch();
			}
		}
		ercd = E_OK;
	}
	else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else {
		runtsk->tstat = (TS_WAITING | TS_WAIT_WOBJ);
		make_wait(&(winfo.winfo));
		queue_insert_prev(&(dtqcb->rwait_queue),
					&(runtsk->task_queue));
		winfo.wobjcb = (WOBJCB *) dtqcb;
		dispatch();
		ercd = winfo.winfo.wercd;
		if (ercd == E_OK) {
			*p_data = winfo.data;
		}
	}
	t_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖からの减慨∈ポ〖リング∷
 */
SYSCALL ER
prcv_dtq(ID dtqid, VP_INT *p_data)
{
	DTQCB	*dtqcb;
	TCB	*tcb;
	VP_INT	data;
	ER	ercd;

	CHECK_TSKCTX_UNL();
	CHECK_DTQID(dtqid);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if (dequeue_data(dtqcb, p_data)) {
		if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
			enqueue_data(dtqcb, data);
			if (wait_complete(tcb)) {
				dispatch();
			}
		}
		ercd = E_OK;
	}
	else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else {
		ercd = E_TMOUT;
	}
	t_unlock_cpu();
	return(ercd);
}

/*
 *  デ〖タキュ〖からの减慨∈タイムアウトあり∷
 */
SYSCALL ER
trcv_dtq(ID dtqid, VP_INT *p_data, TMO tmout)
{
	DTQCB	*dtqcb;
	WINFO_DTQ winfo;
	TMEVTB	tmevtb;
	TCB	*tcb;
	VP_INT	data;
	ER	ercd;

	CHECK_DISPATCH();
	CHECK_DTQID(dtqid);
	CHECK_TMOUT(tmout);
	dtqcb = get_dtqcb(dtqid);

	t_lock_cpu();
	if (dequeue_data(dtqcb, p_data)) {
		if ((tcb = receive_data_swait(dtqcb, &data)) != NULL) {
			enqueue_data(dtqcb, data);
			if (wait_complete(tcb)) {
				dispatch();
			}
		}
		ercd = E_OK;
	}
	else if ((tcb = receive_data_swait(dtqcb, p_data)) != NULL) {
		if (wait_complete(tcb)) {
			dispatch();
		}
		ercd = E_OK;
	}
	else if (tmout == TMO_POL) {
		ercd = E_TMOUT;
	}
	else {
		runtsk->tstat = (TS_WAITING | TS_WAIT_WOBJ);
		make_wait_tmout(&(winfo.winfo), &tmevtb, tmout);
		queue_insert_prev(&(dtqcb->rwait_queue),
					&(runtsk->task_queue));
		winfo.wobjcb = (WOBJCB *) dtqcb;
		dispatch();
		ercd = winfo.winfo.wercd;
		if (ercd == E_OK) {
			*p_data = winfo.data;
		}
	}
	t_unlock_cpu();
	return(ercd);
}