subsystem.c

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

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

/*
 *	subsystem.c (T-Kernel/OS)
 *	Subsystem Manager/Task Exception
 */

#include "kernel.h"
#include "task.h"
#include "check.h"
#include "bitop.h"
#include <sys/rominfo.h>

EXPORT ID	max_ssyid;	/* Maximum subsystem ID */
EXPORT PRI	max_ssypri;	/* Maximum subsystem priority */
EXPORT ID	max_resid;	/* Maximum resource ID */

typedef INT  (*SVC)( VP pk_para, FN fncd );	/* Extended SVC handler */
typedef void (*BrkFN)( ID tskid );		/* Break function */
typedef void (*StuFN)( ID resid, INT info );	/* Startup function */
typedef void (*CluFN)( ID resid, INT info );	/* Cleanup function */
typedef ER   (*EvtFN)( INT evttyp, ID resid, INT info );	/* Event function */

/*
 * Definition of subsystem control block
 */
typedef struct subsystem_control_block	SSYCB;
struct subsystem_control_block {
	SSYCB	*link;		/* Same subsystem priority link */
	ATR	ssyatr;		/* Subsystem attribute */
	PRI	ssypri;		/* Subsystem priority */
	SVC	svchdr;		/* Extended SVC handler */
	BrkFN	breakfn;	/* Break function */
	StuFN	startupfn;	/* Startup function */
	CluFN	cleanupfn;	/* Cleanup function */
	EvtFN	eventfn;	/* Event function */
	INT	resblksz;	/* Resource control block size */
	VP	resblk;		/* Resource control block */
#if TA_GP
	VP	gp;		/* Global pointer */
#endif
};

LOCAL SSYCB	*ssycb_table;	/* Subsystem control block */

#define get_ssycb(id)	( &ssycb_table[INDEX_SSY(id)] )

/*
 * Control table of subsystem priority
 */
LOCAL SSYCB	**ssypri_table;

#define ssypri_index(ssypri)	( (ssypri) - MIN_SSYPRI )

/*
 * Bitmap of use resource ID 
 *	If the bit corresponding to the resource ID is 1, it is in use.
 */
LOCAL UB *resid_bitmap;

/*
 * Convert bitmap index and resource ID
 */
#define index_to_resid(index)	( (index) + MIN_RESID )
#define resid_to_index(resid)	( (resid) - MIN_RESID )

/*
 * Adjust alignment of resource control block
 */
#define ROUND_RESBLKSZ(resblksz)	( ((resblksz) + 3) & ~0x00000003U )

/*
 * Undefined extended SVC function 
 */
IMPORT INT no_support();

/*
 * Initialization of subsystem control block
 */
EXPORT ER subsystem_initialize( void )
{
	INT	i;

	/* Get system information */
	i = _tk_get_cfn(SCTAG_TMAXSSYID, &max_ssyid, 1);
	if ( i < 1 || NUM_SSYID < 1 ) {
		return E_SYS;
	}
	i = _tk_get_cfn(SCTAG_TMAXSSYPRI, &max_ssypri, 1);
	if ( i < 1 || NUM_SSYPRI < 1 ) {
		return E_SYS;
	}

	/* Create subsystem control block */
	ssycb_table = Imalloc((UINT)NUM_SSYID * sizeof(SSYCB));
	if ( ssycb_table == NULL ) {
		goto err_ret1;
	}

	/* Create subsystem priority control table */
	ssypri_table = Imalloc((UINT)NUM_SSYPRI * sizeof(SSYCB*));
	if ( ssypri_table == NULL ) {
		goto err_ret2;
	}

	for ( i = 0; i < NUM_SSYID; i++ ) {
		ssycb_table[i].svchdr    = no_support;
		ssycb_table[i].breakfn   = NULL;
		ssycb_table[i].startupfn = NULL;
		ssycb_table[i].cleanupfn = NULL;
		ssycb_table[i].eventfn   = NULL;
	}

	for ( i = 0; i < NUM_SSYPRI; i++ ) {
		ssypri_table[i] = NULL;
	}

	return E_OK;

err_ret2:
	Ifree(ssycb_table);
err_ret1:
	return E_NOMEM;
}


/*
 * Call break function
 */
IMPORT void call_brkhdr( TCB *tcb );	/* Call break function */


/*
 * Definition of subsystem
 */
SYSCALL ER _tk_def_ssy P2( ID ssid, T_DSSY *pk_dssy )
{
	SSYCB	*ssycb;
	VP	resblk;
	SSYCB	**prev;
	INT	i;
	ER	ercd = E_OK;

	CHECK_SSYID(ssid);
#if CHK_PAR
	if ( pk_dssy != NULL ) {
		CHECK_RSATR(pk_dssy->ssyatr, TA_NULL|TA_GP);
		CHECK_SSYPRI(pk_dssy->ssypri);
		CHECK_PAR(pk_dssy->resblksz >= 0);
	}
#endif

	ssycb = get_ssycb(ssid);

	resblk = NULL;
	if ( pk_dssy != NULL ) {
		if ( pk_dssy->resblksz > 0 ) {
			/* Create resource control block */
			i = ROUND_RESBLKSZ(pk_dssy->resblksz);
			resblk = Icalloc((UINT)NUM_RESID, (UINT)i);
			if ( resblk == NULL ) {
				return E_NOMEM;
			}
		}
	}

	BEGIN_CRITICAL_SECTION;
	if ( pk_dssy != NULL ) {
		/* Register */
		if ( ssycb->svchdr != no_support ) {
			ercd = E_OBJ;  /* Registered */
			goto error_exit;
		}
		ssycb->ssyatr    = pk_dssy->ssyatr;
		ssycb->ssypri    = pk_dssy->ssypri;
		ssycb->svchdr    = (SVC)pk_dssy->svchdr;
		ssycb->breakfn   = (BrkFN)pk_dssy->breakfn;
		ssycb->startupfn = (StuFN)pk_dssy->startupfn;
		ssycb->cleanupfn = (CluFN)pk_dssy->cleanupfn;
		ssycb->eventfn   = (EvtFN)pk_dssy->eventfn;
		ssycb->resblksz  = pk_dssy->resblksz;
		ssycb->resblk    = resblk;
#if TA_GP
		if ( (pk_dssy->ssyatr & TA_GP) != 0 ) {
			gp = pk_dssy->gp;
		}
		ssycb->gp = gp;
#endif

		/* Register to subsystem priority control table */
		i = ssypri_index(ssycb->ssypri);
		ssycb->link = ssypri_table[i];
		ssypri_table[i] = ssycb;
	} else {
		/* Delete */
		if ( ssycb->svchdr == no_support ) {
			ercd = E_NOEXS;  /* Not registered */
			goto error_exit;
		}

		/* Delete from subsystem priority group */
		prev = &ssypri_table[ssypri_index(ssycb->ssypri)];
		while ( *prev != NULL ) {
			if ( *prev == ssycb ) {
				*prev = ssycb->link;
				break;
			}
			prev = &(*prev)->link;
		}

		resblk = ssycb->resblk;
		ssycb->svchdr    = no_support;
		ssycb->breakfn   = NULL;
		ssycb->startupfn = NULL;
		ssycb->cleanupfn = NULL;
		ssycb->eventfn   = NULL;
	}

    error_exit:
	END_CRITICAL_SECTION;

	if ( ercd < E_OK || pk_dssy == NULL ) {
		if ( resblk != NULL ) {
			Ifree(resblk);
		}
	}

	return ercd;
}

/*
 * Call startup function
 */
SYSCALL ER _tk_sta_ssy( ID ssid, ID resid, INT info )
{
	SSYCB	*ssycb;
	StuFN	startupfn;
	UH	save_texflg;
	INT	i;
	ER	ercd = E_OK;

	CHECK_SSYID_ALL(ssid);
	CHECK_RESID(resid);
	CHECK_DISPATCH();

	DISABLE_INTERRUPT;
	/* Suspend task exception while startup function is running */
	save_texflg = ctxtsk->texflg;
	ctxtsk->texflg |= SSFN_RUNNING;
	ctxtsk->sysmode++;
	ENABLE_INTERRUPT;

	if ( ssid > 0 ) {
		/* Call only specified subsystem */
		ssycb = get_ssycb(ssid);
		if ( ssycb->svchdr == no_support ) {
			ercd = E_NOEXS;
		} else {
			startupfn = ssycb->startupfn;
			if ( startupfn != NULL ) {
				CallUserHandlerP2(resid, info,
							startupfn, ssycb);
			}
		}
	} else {
		/* Call all subsystems in the order of descending priotities */
		for ( i = 0; i < NUM_SSYPRI; ++i ) {
			ssycb = ssypri_table[i];
			while ( ssycb != NULL ) {
				startupfn = ssycb->startupfn;
				if ( startupfn != NULL ) {
					CallUserHandlerP2(resid, info,
							startupfn, ssycb);
				}
				ssycb = ssycb->link;
			}
		}
	}

	DISABLE_INTERRUPT;
	ctxtsk->sysmode--;
	ctxtsk->texflg = save_texflg;
	ENABLE_INTERRUPT;

	/* Processing if an exception occurs while suspending a
	   task exception */
	if ( ctxtsk->exectex != 0 ) {
		call_brkhdr(ctxtsk);	/* Execute break function */
	}

	return ercd;
}

/*
 * Clear resource control block
 */
LOCAL void clean_resblk( SSYCB *ssycb, ID idx )
{
	INT	sz;

	if ( ssycb->resblk != NULL ) {
		sz = ROUND_RESBLKSZ(ssycb->resblksz);
		memset((B*)ssycb->resblk + sz * idx, 0, (size_t)sz);
	}
}

/*
 * Call cleanup function
 */
SYSCALL ER _tk_cln_ssy( ID ssid, ID resid, INT info )
{
	SSYCB	*ssycb;
	CluFN	cleanupfn;
	UH	save_texflg;
	INT	i, idx;
	ER	ercd = E_OK;

	CHECK_SSYID_ALL(ssid);
	CHECK_RESID(resid);
	CHECK_DISPATCH();

	DISABLE_INTERRUPT;
	/* Suspend task exception while startup function is running */
	save_texflg = ctxtsk->texflg;
	ctxtsk->texflg |= SSFN_RUNNING;
	ctxtsk->sysmode++;
	ENABLE_INTERRUPT;

	idx = resid_to_index(resid);

	if ( ssid > 0 ) {
		/* Call only specified subsystem */
		ssycb = get_ssycb(ssid);
		if ( ssycb->svchdr == no_support ) {
			ercd = E_NOEXS;
		} else {
			cleanupfn = ssycb->cleanupfn;
			if ( cleanupfn != NULL ) {
				CallUserHandlerP2(resid, info,
							cleanupfn, ssycb);
			}
			clean_resblk(ssycb, idx);
		}
	} else {
		/*  Call all subsystems in the order of ascending priotities */
		for ( i = NUM_SSYPRI-1; i >= 0; --i ) {
			ssycb = ssypri_table[i];
			while ( ssycb != NULL ) {
				cleanupfn = ssycb->cleanupfn;
				if ( cleanupfn != NULL ) {
					CallUserHandlerP2(resid, info,
							cleanupfn, ssycb);
				}
				clean_resblk(ssycb, idx);
				ssycb = ssycb->link;
			}
		}
	}

	DISABLE_INTERRUPT;
	ctxtsk->sysmode--;
	ctxtsk->texflg = save_texflg;
	ENABLE_INTERRUPT;

	/* Processing if an exception occurs while suspending a
	   task exception */
	if ( ctxtsk->exectex != 0 ) {
		call_brkhdr(ctxtsk);	/* Execute break function */
	}

	return ercd;
}

/*
 * Call event function
 */
SYSCALL ER _tk_evt_ssy( ID ssid, INT evttyp, ID resid, INT info )
{
	SSYCB	*ssycb;
	EvtFN	eventfn;
	UH	save_texflg;
	INT	i, e, d;
	ER	ercd = E_OK;
	ER	er = E_OK;

	CHECK_SSYID_ALL(ssid);
	CHECK_RESID_ANY(resid);
	CHECK_DISPATCH();

	DISABLE_INTERRUPT;
	/* Suspend task exception while startup function is running */
	save_texflg = ctxtsk->texflg;
	ctxtsk->texflg |= SSFN_RUNNING;
	ctxtsk->sysmode++;
	ENABLE_INTERRUPT;

	if ( ssid > 0 ) {
		/* Call only specified subsystem */
		ssycb = get_ssycb(ssid);
		if ( ssycb->svchdr == no_support ) {
			ercd = E_NOEXS;
		} else {
			eventfn = ssycb->eventfn;
			if ( eventfn != NULL ) {
				ercd = CallUserHandlerP3(evttyp, resid, info,
							eventfn, ssycb);
			}
		}
	} else {
		if ( (evttyp % 2) == 0 ) {
			/* Even number:  Call all in the order of
			   ascending priotities */
			i = NUM_SSYPRI-1;
			e = -1;
			d = -1;
		} else {
			/* Odd number: Call all in the order of
			   descending priotities */
			i = 0;
			e = NUM_SSYPRI;
			d = +1;
		}
		for ( ; i != e; i += d ) {
			ssycb = ssypri_table[i];
			while ( ssycb != NULL ) {
				eventfn = ssycb->eventfn;
				if ( eventfn != NULL ) {
					er = CallUserHandlerP3(evttyp,resid,info,
							eventfn, ssycb);
					if ( er < 0 ) {	/* Set Latest Error	*/
						ercd = er;	/* Overwrite!	*/
					}
				}
				ssycb = ssycb->link;
			}
		}
	}

	DISABLE_INTERRUPT;
	ctxtsk->sysmode--;
	ctxtsk->texflg = save_texflg;
	ENABLE_INTERRUPT;

	/* Processsing if an exception occurs while suspending a
	   task exception */
	if ( ctxtsk->exectex != 0 ) {
		call_brkhdr(ctxtsk);	/* Execute break function */
	}

	return ercd;
}

/*
 * Refer subsystem definition information
 */
SYSCALL ER _tk_ref_ssy( ID ssid, T_RSSY *pk_rssy )
{
	SSYCB	*ssycb;
	ER	ercd = E_OK;

	CHECK_SSYID(ssid);

	ssycb = get_ssycb(ssid);

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

	return ercd;
}

#if USE_DBGSPT

/*
 * Refer subsystem usage state
 */
SYSCALL INT _td_lst_ssy( ID list[], INT nent )
{
	SSYCB	*ssycb, *end;
	INT	n = 0;

	BEGIN_DISABLE_INTERRUPT;
	end = ssycb_table + NUM_SSYID;
	for ( ssycb = ssycb_table; ssycb < end; ssycb++ ) {
		if ( ssycb->svchdr == no_support ) {
			continue;
		}

		if ( n++ < nent ) {
			*list++ = ID_SSY(ssycb - ssycb_table);
		}
	}
	END_DISABLE_INTERRUPT;

	return n;
}

/*
 * Refer subsystem definition information
 */
SYSCALL ER _td_ref_ssy( ID ssid, TD_RSSY *pk_rssy )
{
	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;