subsystem.c

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

{
	SSYCB	*ssycb;
	ER	ercd = E_OK;

	CHECK_SSYID(ssid);

	ssycb = get_ssycb(ssid);

	BEGIN_DISABLE_INTERRUPT;
	if ( ssycb->svchdr == no_support ) {
		ercd = E_NOEXS;
	} else {
		pk_rssy->ssypri   = ssycb->ssypri;
		pk_rssy->resblksz = ssycb->resblksz;
	}
	END_DISABLE_INTERRUPT;

	return ercd;
}
#endif /* USE_DBGSPT */

/*
 * Branch routine to extended SVC handler
 */
EXPORT ER svc_ientry P2( VP pk_para, FN fncd )
{
	ID	ssid;
	SSYCB	*ssycb;
	ID	save_execssid;
	UINT	save_waitmask;
	UINT	save_exectex;
	ER	ercd;

	/* Lower 8 bits are subsystem ID */
	ssid = fncd & 0xff;
	if ( ssid < 1 || ssid > MAX_SSYID ) return E_RSFN;

	ssycb = get_ssycb(ssid);

	if ( in_indp() ) {
		/* Execute at task-independent part */
#if TA_GP
		ercd = CallUserHandler((INT)pk_para, fncd, (INT)gp,
					(FP)ssycb->svchdr, ssycb->gp);
#else
		ercd = (*ssycb->svchdr)(pk_para, fncd);
#endif
	} else {
		/* Lock to run with break function exclusively */
		LockSVC(&ctxtsk->svclock);

		if ( (ctxtsk->waitmask & TTX_SVC) != 0 ) {
			UnlockSVC();
			return E_DISWAI; /* Disable extended SVC call */
		}

		DISABLE_INTERRUPT;
		save_execssid = ctxtsk->execssid;
		save_waitmask = ctxtsk->waitmask;
		save_exectex  = ctxtsk->exectex;
		ctxtsk->execssid = ssid;
		ctxtsk->waitmask = 0;
		/* If the break function was already called,
		   clear the task exception to avoid to run the
		   extended SVC break function at called extended SVC */
		if ( save_execssid < 0 ) ctxtsk->exectex = 0;
		ctxtsk->sysmode++;
		ENABLE_INTERRUPT;

		UnlockSVC();

		/* Call extended SVC handler */
#if TA_GP
		ercd = CallUserHandler((INT)pk_para, fncd, (INT)gp,
					(FP)ssycb->svchdr, ssycb->gp);
#else
		ercd = (*ssycb->svchdr)(pk_para, fncd);
#endif

		/* Lock in order to run with break function exclusively */
		LockSVC(&ctxtsk->svclock);

		DISABLE_INTERRUPT;
		ctxtsk->sysmode--;
		ctxtsk->execssid = save_execssid;
		ctxtsk->waitmask = save_waitmask;
		ctxtsk->exectex |= save_exectex;
		ENABLE_INTERRUPT;

		UnlockSVC();

		if ( ctxtsk->exectex != 0 ) {
			call_brkhdr(ctxtsk);	/* Execute break function */
		}
	}

	return ercd;
}

/* ------------------------------------------------------------------------ */
/*
 *	Task exception function
 */

/*
 * Definition of task exception handler 
 */
SYSCALL ER _tk_def_tex( ID tskid, T_DTEX *pk_dtex )
{
	TCB	*tcb;
	ER	ercd = E_OK;

	CHECK_TSKID_SELF(tskid);
#ifdef CHK_PAR
	if ( pk_dtex != NULL ) {
		CHECK_RSATR(pk_dtex->texatr, TA_NULL);
	}
#endif

	tcb = get_tcb_self(tskid);

	BEGIN_CRITICAL_SECTION;
	if ( tcb->state == TS_NONEXIST ) {
		ercd = E_NOEXS;
		goto error_exit;
	}
	if ( (tcb->tskatr & TA_RNG3) == TA_RNG0 ) {
		ercd = E_OBJ;
		goto error_exit;
	}

	/* Initialize task exception information */
	tcb->pendtex = 0;
	tcb->exectex = 0;
	tcb->texmask = 0;
	tcb->texhdr  = ( pk_dtex != NULL )? pk_dtex->texhdr: NULL;

    error_exit:
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Enable/Disable task exception 
 */
LOCAL ER set_tex_mask( ID tskid, UINT texmsk, BOOL enable )
{
	TCB	*tcb;
	ER	ercd = E_OK;

	CHECK_TSKID_SELF(tskid);

	tcb = get_tcb_self(tskid);

	BEGIN_CRITICAL_SECTION;
	if ( tcb->state == TS_NONEXIST || tcb->texhdr == NULL ) {
		ercd = E_NOEXS;
		goto error_exit;
	}

	if ( enable )	tcb->texmask |= texmsk;		/* Enable */
	else		tcb->texmask &= ~texmsk;	/* Disable */

	/* Narrow down to only the enabled task exception that is suspended */
	tcb->pendtex &= tcb->texmask;

    error_exit:
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Disable task exception
 */
SYSCALL ER _tk_dis_tex( ID tskid, UINT texptn )
{
	return set_tex_mask(tskid, texptn, FALSE);
}

/*
 * Enable task exception
 */
SYSCALL ER _tk_ena_tex( ID tskid, UINT texptn )
{
	return set_tex_mask(tskid, texptn, TRUE);
}


/*
 * Call break function
 *	Call break function for extended SVC which 'tcb' task is executing. 
 */
EXPORT void call_brkhdr( TCB *tcb )
{
	SSYCB	*ssycb;
	BFN	breakfn = NULL;
	UH	save_texflg;
	INT	priority;
#if TA_GP
	VP	ssy_gp;
#endif

	BEGIN_CRITICAL_SECTION;
	/* If subsystem function (Startup function, Cleanup function, 
	   Event function/Break function} are running, suspend the
	   task exception. */
	if ( (tcb->texflg & SSFN_RUNNING) != 0 ) goto not_call;

	/* When the task exception occurred with extended SVC running, 
	   call break function only once */
	if ( !(tcb->exectex != 0 && tcb->execssid > 0) ) goto not_call;

	/* Extended SVC (subsystem) in execution */
	ssycb = get_ssycb(tcb->execssid);

	tcb->execssid |= BREAK_RAN;  /* Executed mark: Disable multiple call */

	breakfn = ssycb->breakfn;
	if ( breakfn == NULL ) goto not_call;  /* Break function is not set */
#if TA_GP
	ssy_gp = ssycb->gp;
#endif

	if ( tcb->priority < ctxtsk->priority ) {
		/* Raise its own task priority to the same level of the task 
		   running extended SVC */
		change_task_priority(ctxtsk, tcb->priority);
	}

	/* Suspend the task exception during break function is running */
	save_texflg = ctxtsk->texflg;
	ctxtsk->texflg |= SSFN_RUNNING;
	ctxtsk->sysmode++;

    not_call:
	END_CRITICAL_SECTION;
	if ( breakfn == NULL ) return;  /* Do not call break function */

	/* Call break function */
#if TA_GP
	CallUserHandler(tcb->tskid, 0, 0, breakfn, ssy_gp);
#else
	(*breakfn)(tcb->tskid);
#endif

	BEGIN_CRITICAL_SECTION;
	ctxtsk->sysmode--;
	ctxtsk->texflg = save_texflg;

	/* Set the task priority back to the original level */
#ifdef NUM_MTXID
	priority = chg_pri_mutex(ctxtsk, ctxtsk->bpriority);
#else
	priority = ctxtsk->bpriority;
#endif
	if ( ctxtsk->priority != priority ) {
		change_task_priority(ctxtsk, priority);
	}
	END_CRITICAL_SECTION;
}


/*
 * Raise task exception
 */
SYSCALL ER _tk_ras_tex( ID tskid, INT texcd )
{
	TCB	*tcb;
	BOOL	rastex = FALSE;
	ER	ercd = E_OK;

	CHECK_TSKID_SELF(tskid);
	CHECK_PAR(texcd >= 0 && texcd <= 31);
	CHECK_DISPATCH();

	tcb = get_tcb_self(tskid);

	/* Lock in order to run with extended SVC handler exclusively */
	LockSVC(&tcb->svclock);

	BEGIN_CRITICAL_SECTION;
	if ( tcb->state == TS_NONEXIST || tcb->texhdr == NULL ) {
		ercd = E_NOEXS;
		goto error_exit;
	}
	if ( tcb->state == TS_DORMANT ) {
		ercd = E_OBJ;
		goto error_exit;
	}

	/* Ignore exceptions if '0' exception code handler is in execution */
	if ( (tcb->texflg & TEX0_RUNNING) != 0 ) goto error_exit;

	tcb->pendtex |= (1 << texcd) & tcb->texmask;
	if ( tcb->pendtex == 0 ) goto error_exit; /* No exception occurred */

	/* The exception handler can be nested only when the exception
	   code is 0.
	   In other cases, if the exception handler is running, suspend it. */
	if ( (tcb->pendtex & 0x00000001) != 0
	  || (tcb->texflg & TEX1_RUNNING) == 0 ) {
		/* Task exception occurred */
		tcb->exectex |= tcb->pendtex;
		rastex = TRUE;

		/* Request task exception handler execution */
		request_tex(tcb);
	}

    error_exit:
	END_CRITICAL_SECTION;

	if ( rastex ) {
		/* Execute break function */
		call_brkhdr(tcb);
	}

	UnlockSVC();

	return ercd;
}

/*
 * End task exception handler 
 */
SYSCALL INT _tk_end_tex( BOOL enatex )
{
	INT	texcd = 0;
	UINT	texptn;

	CHECK_DISPATCH();
	if ( (ctxtsk->texflg & (TEX0_RUNNING|TEX1_RUNNING)) != TEX1_RUNNING )
							return E_CTX;

	BEGIN_CRITICAL_SECTION;
	texptn = ctxtsk->pendtex & ~1;  /* Except exception code 0 */
	if ( texptn != 0 ) {
		while ( (texptn & 1) == 0 ) {
			texcd++;
			texptn >>= 1;
		}
	}

	/* If enatex = TRUE or a task exception didn't occur,
	   enable the task exception after executing the task
	   exception handler  */
	if ( enatex || texcd == 0 ) {
		ctxtsk->texflg &= ~TEX1_RUNNING;
		ctxtsk->exectex |= ctxtsk->pendtex;

		if ( texcd > 0 ) {
			/* Since there is a task exception suspension,
			   request the task execution */
			request_tex(ctxtsk);
		}
	} else {
		/* Continue task exception handler execution */
		ctxtsk->pendtex &= ~(1 << texcd);
	}
	END_CRITICAL_SECTION;

	return texcd;
}

/*
 * Refer task exception state
 */
SYSCALL ER _tk_ref_tex( ID tskid, T_RTEX *pk_rtex )
{
	TCB	*tcb;
	ER	ercd = E_OK;

	CHECK_TSKID_SELF(tskid);

	tcb = get_tcb_self(tskid);

	BEGIN_CRITICAL_SECTION;
	if ( tcb->state == TS_NONEXIST ) {
		ercd = E_NOEXS;
	} else {
		pk_rtex->pendtex = tcb->pendtex;
		pk_rtex->texmask = tcb->texmask;
	}
	END_CRITICAL_SECTION;

	return ercd;
}

#if USE_DBGSPT
/*
 * Refer task exception state
 */
SYSCALL ER _td_ref_tex( ID tskid, TD_RTEX *pk_rtex )
{
	TCB	*tcb;
	ER	ercd = E_OK;

	CHECK_TSKID_SELF(tskid);

	tcb = get_tcb_self(tskid);

	BEGIN_DISABLE_INTERRUPT;
	if ( tcb->state == TS_NONEXIST ) {
		ercd = E_NOEXS;
	} else {
		pk_rtex->pendtex = tcb->pendtex;
		pk_rtex->texmask = tcb->texmask;
	}
	END_DISABLE_INTERRUPT;

	return ercd;
}
#endif /* USE_DBGSPT */

/* ------------------------------------------------------------------------ */
/*
 *	Resource group management
 */

/*
 * Initialization of resource group management
 */
EXPORT ER resource_group_initialize( void )
{
	W	n;

	/* Get system information */
	n = _tk_get_cfn("TMaxResId", &max_resid, 1);
	if ( n < 1 || NUM_RESID < 1 ) return E_SYS;

	/* Create bitmap of resource ID */
	resid_bitmap = Icalloc(1, (NUM_RESID + 7) / 8);
	if ( resid_bitmap == NULL ) return E_NOMEM;

	/* System resource always exist */
	tstdlib_bitset(resid_bitmap, resid_to_index(SYS_RESID));

	return E_OK;
}

/*
 * Create resource group
 */
SYSCALL ID _tk_cre_res( void )
{
	INT	n;
	ER	ercd;

	BEGIN_CRITICAL_SECTION;
	n = tstdlib_bitsearch0(resid_bitmap, 0, NUM_RESID);
	if ( n < 0 ) {
		ercd = E_LIMIT;
	} else {
		tstdlib_bitset(resid_bitmap, n);
		ercd = index_to_resid(n);
	}
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Delete resource group
 */
SYSCALL ER _tk_del_res( ID resid )
{
	INT	idx;
	ER	ercd = E_OK;

	CHECK_RESID(resid);
	if ( resid == SYS_RESID ) return E_ID;

	BEGIN_CRITICAL_SECTION;
	idx = resid_to_index(resid);
	if ( !tstdlib_bittest(resid_bitmap, idx) ) {
		ercd = E_NOEXS;
	} else {
		tstdlib_bitclr(resid_bitmap, idx);
	}
	END_CRITICAL_SECTION;

	return ercd;
}

/*
 * Get resource control block
 */
SYSCALL ER _tk_get_res( ID resid, ID ssid, VP *p_resblk )
{
	INT	idx;
	SSYCB	*ssycb;
	ER	ercd = E_OK;

	CHECK_RESID(resid);
	CHECK_SSYID(ssid);

	idx = resid_to_index(resid);
	ssycb = get_ssycb(ssid);

	BEGIN_CRITICAL_SECTION;
	if ( !tstdlib_bittest(resid_bitmap, idx)
	  || ssycb->svchdr == no_support ) {
		ercd = E_NOEXS;
		goto error_exit;
	}

	/* Resource control block address*/
	*p_resblk = (B*)ssycb->resblk + ROUND_RESBLKSZ(ssycb->resblksz) * idx;

    error_exit:
	END_CRITICAL_SECTION;

	return ercd;
}