azrtos.c

this is RealTimeOS(uITRON) for hitachi H8 processor.

/*
 *	azrtos.c - AzkiRTOS Implementation
 *	Copyright 1999-2001, Anchor Systems Corporation
 *	All rights reserved.
 *
 *	偙偺僼傽僀儖偼丄AzkiRTOS偺堦晹暘偱偡丅
 *	偙偺僼傽僀儖偺偺挊嶌尃偼傾儞僇乕僔僗僥儉僘姅幃夛幮偵懏偟傑偡偑丄
 *	扤偱傕僼傽僀儖傪柍曄峏丄傑偨偼曄峏偟偰帺桼偵巊偆偙偲偑偱偒傑偡丅
 *	傑偨柍彏偱嵞攝晍偡傞偙偲傕壜擻偱偡偑丄僆儕僕僫儖偺挊嶌尃昞帵傪
 *	偦偺傑傑偺宍偱娷傔偰偔偩偝偄丅
 *
 *	偨偩偟丄柍曄峏傑偨偼曄峏偟偨傕偺傪惢昳偺堦晹偲偟偰偱偼側偔丄
 *	儕傾儖僞僀儉OS偦偺傕偺偲偟偰柍抐偱嵞斕攧偡傞偙偲偼偱偒傑偣傫丅
 *
 *	傾儞僇乕僔僗僥儉僘(姅)偼丄柍曄峏傑偨偼曄峏偟偨AzkiRTOS傪慻傒崬傫偩
 *	惢昳偺晄嬶崌偵懳偟偰堦愗愑擟傪晧偆傕偺偱偼偁傝傑偣傫丅AzkiRTOS傪
 *	娷傔AzkiRTOS傪慻傒崬傫偩惢昳慡懱偺摦嶌曐徹偼丄AzkiRTOS傪慻傒崬傫偩
 *	惢昳偺奐敪幰偺愑擟偲側傝傑偡丅
 *
 *	$Revision: 27 $
 *	$Archive: /AnchorPlace/goodies/AzRTOS/azrtos.c $
 */

#include "azrtos.h"
#include "azpriv.h"
#include <stdlib.h>


/****************************************************************************
 * AzkiRTOS - 僔僗僥儉僨乕僞
 ****************************************************************************/

AzLink	az_ready[AZ_NPRIORITIES];	/* READY 僉儏乕(桪愭弴埵偺悢偩偗偁傞) */
AzLink	az_timer;					/* 帪娫懸偪/僞僀儉傾僂僩僉儏乕 */
AzTCB*	az_curtcb;					/* 尰嵼RUN忬懺偺僞僗僋 */
UB		az_dsptsk;					/* 僨傿僗僷僢僠昁梫僼儔僌 0=晄梫, 1=昁梫, 2=嬛巭, 3=昁梫偩偗偳嬛巭 */
UB		az_work;					/* 堦帪巊梡椞堟 (M16C摿桳) */
SYSTIME	az_systime;					/* 僔僗僥儉帪娫 */

AzTCB	az_idle;					/* 傾僀僪儖僞僗僋(嵟弶偵婲摦偝傟傞僞僗僋)偺TCB */
UB		az_stack[AZ_IDLESTACKSIZE];	/* 傾僀僪儖僞僗僋(嵟弶偵婲摦偝傟傞僞僗僋)偺僗僞僢僋 */
extern void AZ_IDLEENTRY(void);		/* 傾僀僪儖僞僗僋(嵟弶偵婲摦偝傟傞僞僗僋)擖傝岥娭悢偺僾儘僩僞僀僾 */


/****************************************************************************
 * AzkiRTOS - 僿儖僷乕娭悢偲儅僋儘
 ****************************************************************************/

#define az_schedule_dispatch()		(az_dsptsk |=  1)
#define az_cancel_dispatch()		(az_dsptsk &= ~1)
#define az_enable_dispatch()		(az_dsptsk &= ~2)
#define az_disable_dispatch()		(az_dsptsk |=  2)
#define is_dispatch_disabled()		(az_dsptsk & 2)
void	az_dispatch(void);

void	az_init(void);
void	az_insert_ready(AzTCB* tcb);
#define	az_delete_ready(tcb)		az_unlink_node(&(tcb)->m_mainlink)
#define	az_store_result(tcb, ercd)	(*((ER*)(tcb)->m_usp) = (ercd))


/****************************************************************************
 * AzkiRTOS - 僞僀儅TA0偺惂屼儗僕僗僞傾僪儗僗
 ****************************************************************************/

#if AZ_TIMERINTERVAL > 0
#define TA0		0386H				/* TA0 僇僂儞僞儗僕僗僞 */
#define TA0MR	0396H				/* TA0 儌乕僪儗僕僗僞 */
#define TA0IC	0055H				/* TA0 妱傝崬傒惂屼儗僕僗僞 */
#define TABSR	0380H				/* 僇僂儞僩奐巒僼儔僌儗僕僗僞 */
#endif /* AZ_TIMERINTERVAL */


/****************************************************************************
 * AzkiRTOS - 弶婜壔
 ****************************************************************************/

void vini_sys(void)
{
	/* 娭悢僐乕儖傪僉儍儞僙儖偡傞
	 (-genter 僆僾僔儑儞傪巜掕偟偨帪偼 add.w #5,sp 偵側傞偑2僶僀僩偺柍懯偱嵪傓偨傔
	 偙偙偱偼#3偵偟偰偍偔丅乯  */
	TRACE("vini_sys()");
	_asm("add.w #3, sp");

	/* AzkiRTOS 偺曄悢傪弶婜壔偟丄 */
	az_init();

	/* 昁梫側傜僞僀儅傪巒摦偟偰偐傜丄 */
#if AZ_TIMERINTERVAL > 0
#pragma ASM
	/* 僞僀儅乕 TA0 傪僞僀儅儌乕僪 1/32 CLK丄妱傝崬傒儗儀儖1丄僀儞僞乕僶儖
	   AZ_TIMERINTERVAL msec 偱僇僂儞僩奐巒偡傞 */
	mov.b	#10000000B, TA0MR
	mov.w	#(AZ_CPUCLOCK / 32 * AZ_TIMERINTERVAL) - 1, TA0
	mov.b	#1, TA0IC
	or.b	#1, TABSR
#pragma ENDASM
#endif /* AZ_TIMERINTERVAL */

	/* 嵟弶偺僞僗僋傊旘傃崬傓 */
	TRACE("Jump into the first task.");
	az_dispatch();
}

void az_init(void)
{
	/* AzkiRTOS慡懱傪弶婜壔偡傞 */
	int i;
	for (i = 0; i < arraysizeof(az_ready); i++)
		az_init_list(&az_ready[i]);

	az_init_list(&az_timer);
	az_curtcb = NULL;
	az_dsptsk = 0;
	az_work = 0;

	/* 僔僗僥儉帪娫偺僇僂儞僩偼0偐傜弌敪 */
	az_systime.m_low = 0;
	az_systime.m_mid = 0;
	az_systime.m_high = 0;

	/* 嵟弶偺傾僀僪儖僞僗僋偺TCB傪弨旛偟READY忬懺偵偡傞 */
	vini_tsk(&az_idle, AZ_IDLEENTRY, AZ_NPRIORITIES, az_stack, arraysizeof(az_stack));
	az_setup_tcb(&az_idle, 0);
	az_insert_ready(&az_idle);
}

void vini_tsk(ID tskid, FP entry, PRI tskpri, void* stack, UINT size)
{
	/* TCB傪弶婜壔偡傞 */
	AzTCB* tcb = tskid;
	ASSERT(tcb);

	tcb->m_id			= 0;
	tcb->m_attr			= 0;
	tcb->m_initpri		= tskpri - 1;
	tcb->m_state		= TTS_DMT;
	tcb->m_entry		= entry;
	tcb->m_stackarea	= stack;
	tcb->m_stacksize	= size;
}

void vini_sem(ID semid, INT inival)
{
	/* 僙儅僼僅傪弶婜壔偡傞 */
	AzSemaphore* sem = semid;
	ASSERT(sem);

	az_init_list(&sem->m_queue);
	sem->m_count	= inival;
	sem->m_id		= 0;
	sem->m_attr		= 0;
}


/****************************************************************************
 * AzkiRTOS - 僞僗僋暿 errno 傊偺傾僋僙僗娭悢
 ****************************************************************************/

int _near * az_errno()
{
	static s_errno = 0;
	UH flag;
	ID tskid;

	/* 僞僗僋撈棫晹偐傜偺errno嶲徠偐? */
	_asm("stc  flg, $$[fb]", flag);
	if ((flag & 0x0080) == 0)
		return &s_errno;

	/* 奺僞僗僋偐傜偺嶲徠側傜丄TCB撪偺m_errno傪曉偡 */
	VERIFY(get_tid(&tskid) == E_OK);
	return &((AzTCB*) tskid)->m_errno;
}


/****************************************************************************
 * AzkiRTOS - INT #32 僔僗僥儉僐乕儖偺擖傝岥
 ****************************************************************************/

ER		az_sta_tsk(AzTCB* tcb, INT stacd);
void	az_ext_tsk(void);
ER		az_ter_tsk(AzTCB* tcb);
ER		az_dis_dsp(void);
ER		az_ena_dsp(void);
ER		az_chg_pri(AzTCB* tcb, PRI tskpri);
ER		az_rel_wai(AzTCB* tcb);
ER		az_get_tid(AzTCB** p_tskid);
ER		az_ref_tsk(T_RTSK* pk_rtsk, AzTCB* tcb);
ER		az_tslp_tsk(TMO tmout);
ER		az_wup_tsk(AzTCB* tcb);
ER		az_can_wup(INT* p_wupcnt, AzTCB* tcb);
ER		az_twai_sem(AzSemaphore* sem, TMO tmout);
ER		az_dly_tsk(DLYTIME dlytim);

ER		az_release_wait(AzTCB* tcb, BOOL bSyscall);
ER		az_wakeup_task(AzTCB* tcb, BOOL bSyscall);
ER		az_change_priority(AzTCB* tcb, PRI tskpri, BOOL bSyscall);
ER		az_reference_task(T_RTSK* pk_rtsk, AzTCB* tcb, BOOL bSyscall);

const FP sys_funcs[] =
{
	(FP) az_sta_tsk,	/*  0 */
	(FP) az_ext_tsk,	/*  1 */
	(FP) az_ter_tsk,	/*  2 */
	(FP) az_dis_dsp,	/*  3 */
	(FP) az_ena_dsp,	/*  4 */
	(FP) az_chg_pri,	/*  5 */
	(FP) irot_rdq,		/*  6 */
	(FP) az_rel_wai,	/*  7 */
	(FP) az_get_tid,	/*  8 */
	(FP) az_ref_tsk,	/*  9 */
	(FP) az_tslp_tsk,	/* 10 */
	(FP) az_wup_tsk,	/* 11 */
	(FP) az_can_wup,	/* 12 */
	(FP) isig_sem,		/* 13 */
	(FP) az_twai_sem,	/* 14 */
	(FP) iref_sem,		/* 15 */
	(FP) iset_tim,		/* 16 */
	(FP) iget_tim,		/* 17 */
	(FP) az_dly_tsk,	/* 18 */
};

#define AZ_LASTFUNCNUM		18


/* ----------------------------------------------------------------------
 * NOTE:
 *	偙偺娭悢偼 INT #32 偺僴儞僪儔偱丄儐乕僓僗僞僢僋偐傜妱傝崬傒僗僞僢僋傊
 *	愗傝懼偊偰偐傜奺婡擻僐乕僪傪張棟偡傞偨傔偺娭悢傪屇傃弌偡丅偙偺傛偆側
 *	娭悢偼傾僙儞僽儕尵岅偺曽偑慺捈偵婰弎偱偒傞丅儕僞乕儞偡傞偲偒偼丄捈愙
 *	reit傪僐乕僨傿儞僌偟偰偄傞偑丄僨傿僗僷僢僠偑昁梫偵側偭偨応崌偼丄偙偙
 *	偐傜偼儕僞乕儞偟側偄丅
 * ---------------------------------------------------------------------*/


void az_syscall(void)
{
	/* ----------------------------------------------------------------------
	 * NOTE:
	 *	NC30偱偼丄C尵岅娭悢偺宍傪偲傞偲丄-genter 偺僆僾僔儑儞愝掕偵傛傝 堷悢偺
	 *	側偄娭悢偱 enter柦椷偺惗惉偵嵎偑弌傞偨傔僗僞僢僋偺忬懺偑曄壔偟偰偟傑偆丅
	 *  偦偙偱娭悢偦偺傕偺傪傾僙儞僽儕尵岅偱婰弎偡傞丅乮az_retint傕摨偠乯
	 *----------------------------------------------------------------------*/
#pragma ASM
	.glb	__az_syscall
__az_syscall:
	/* 嵟弶偵僔僗僥儉僐乕儖偺婡擻僐乕僪斖埻傪僠僃僢僋偡傞 */
	cmp.w	#AZ_LASTFUNCNUM, r0
	jgtu	bad_parameter

	/* 尰嵼偺儗僕僗僞傪僞僗僋偺僗僞僢僋偵曐懚偟丄妱傝崬傒僗僞僢僋傊愗傝懼偊傞 */
	pushm	r1, r2, r3, a0, a1, fb
	fclr	u
	fset	i

	/* 僔僗僥儉僐乕儖偺奺張棟傪僐乕儖偡傞(Jump Table曽幃)
	 * ----------------------------------------------------------------------
	 * NOTE:
	 *	AzkiRTOS/M16C偺僔僗僥儉僐乕儖偼丄嵟戝偱傕16-bit偺堷悢傪2偮傑偱側偺偱丄
	 *	僜僼僩僂僃傾妱傝崬傒帪揰偱 R1丄R2 偵擖傟丄儗僕僗僞搉偟偑壜擻偱偁傞丅
	 *	傑偨屄乆偺僔僗僥儉僐乕儖傪張棟偡傞娭悢偱傕丄僾儘僩僞僀僾愰尵傪偡傞
	 *	偡傞偙偲偱丄R1丄R2儗僕僗僞搉偟偲側傞丅廬偭偰偙偙偱偼丄堷悢傪搉偡偨
	 *	傔偵僗僞僢僋傪憖嶌偡傞昁梫偑堦愗側偔側傝丄偨偩俠偱婰弎偟偰偁傞屄乆
	 *	偺娭悢傪屇傃弌偡偩偗偱傛偄丅傕偪傠傫僐儞僷僀儔傪曄偊偨傝丄懠偺CPU
	 *	傊堏怉偡傞応崌偵偼丄偦傟偵崌傢偣偰挷惍偡傞昁梫偑偁傞丅
	 *----------------------------------------------------------------------*/
	stc		sp, a0
	ldc		a0, fb
	shl.w	#2, r0
	mov.w	r0, a0
	jsri.a	_sys_funcs[a0]

	/* 嵞傃妱傝崬傒嬛巭偲偟偰丄僨傿僗僷僢僠偑昁梫偐偳偆偐挷傋傞 */
	fclr	i
	nop
	cmp.b	#1, _az_dsptsk
	jz		do_dispatch

	/* 僨傿僗僷僢僠晄梫側傜丄儗僕僗僞傪億僢僾偟偰儐乕僓忬懺傊儕僞乕儞偡傞 */
	fset	u
	popm	r1, r2, r3, a0, a1, fb
	reit

bad_parameter:
	/* 婡擻僐乕僪僄儔乕 */
	mov.w	#E_PAR, r0
	reit

do_dispatch:
	/* ----------------------------------------------------------------------
	 *	僨傿僗僷僢僠偡傞偺偱偁傟偽丄嵞傃RUN忬懺偵暅婣偡傞偲偒偵偙偺僔僗僥儉
	 *	僐乕儖偺栠傝抣偑庢傝弌偣傞傛偆儐乕僓僗僞僢僋傊奿擺偟偰偐傜丄僨傿僗
	 *	僷僢僠儖乕僠儞傊揤壓屼柶偺僕儍儞僾傪偡傞丅偨偲偊僔僗僥儉僐乕儖偺寢壥
	 *	偑僄儔乕(E_OK偱側偄)偱偁偭偰傕丄僔僗僥儉僐乕儖拞偵敪惗偟偨僴乕僪妱傝
	 *	崬傒偺拞偱偺僨傿僗僷僢僠梫媮偑抶墑偝傟偰偄傞偐傕偟傟側偄偺偱丄寢壥偺
	 *	擛壗偵娭傢傜偢僨傿僗僷僢僠偡傞昁梫偑偁傞丅
	 * ----------------------------------------------------------------------
	 * NOTE:
	 *	M16C偺応崌丄偨傑偨傑僔僗僥儉僐乕儖偺栠傝抣偺傾僪儗僗(僐儞僥僉僗僩傪
	 *	嵞儘乕僪偡傞帪R0偺抣偑曐懚偝傟偰偄傞儊儌儕傾僪儗僗)偼儐乕僓僗僞僢僋
	 *	億僀儞僞偑巜偟偰偄傞傾僪儗僗偵堦抳偡傞丅偦偺偨傔丄栠傝偺傾僪儗僗傪
	 *	TCB偵傢偞傢偞帩偮偙偲偼偟側偄丅僞僀儉傾僂僩傗 rel_wai() 側偳偱懸偪
	 *	忬懺偑嫮惂夝彍偝傟偨応崌偱傕丄TCB偺儐乕僓僗僞僢僋億僀儞僞偑帵偟偰偄傞
	 *	傾僪儗僗傊懸偪忬懺偑拞抐偝傟偨偙偲傪抦傜偣傞僄儔乕僐乕僪 E_TMOUT 傗
	 *	E_RLWAI 傪彂偒崬傔偽丄懸偪忬懺偵擖偭偨帪偵曐懚偟偨抣(偍偦傜偔E_OK)偲
	 *	梕堈偵偡傝懼偊傞偙偲偑偱偒傞丅偙偺曽朄偼丄懠偺僐儞僷僀儔傗CPU偱偼
	 *	巊偊側偄偐傕抦傟側偄偺偱丄偦偺帪偼TCB拞偵愱梡偺椞堟傪愝偗傞偙偲丅
	 * --------------------------------------------------------------------*/
	fset	u
	push.w	r0
	fclr	u
	jsr.w	_az_dispatch
#pragma ENDASM
	/* 偙偙傊偼栠偭偰棃側偄 */
}


/****************************************************************************
 * AzkiRTOS - INT #34 ret_int 僔僗僥儉僐乕儖偺擖傝岥
 ****************************************************************************/

void az_retint(void)
{
#pragma ASM
	.glb	__az_retint
__az_retint:

	/* INT #34 偼丄妱傝崬傒僴儞僪儔偐傜偺儕僞乕儞張棟傪峴偆丅
	 * 偙偺娭悢偼丄妱傝崬傒僗僞僢僋傪巊偭偰摦嶌偟丄妱崬傒嬛巭忬懺偱偁傞丅
	 * -----------------------------------------------------------------------
	 * WARNING:
	 *	INT #34 偼丄#pragma INTHANDLER偱愰尵偟偨娭悢偺嵟屻丄偡傋偰偺儗僕僗僞傪
	 *	億僢僾偟偨偁偲偐傜屇偽傟傞偨傔丄FLG 傪彍偔儗僕僗僞抣傪曄峏偡傞偙偲偑偱
	 *	偒側偄丅
	 * ----------------------------------------------------------------------
	 * INT #34 屇傃弌偟傪僉儍儞僙儖偡傞丅
	 * 埲壓偵丄偙偺帪揰偱偺僗僞僢僋偺忬懺傪恾帵偡傞丅
	 *
	 *	ISP--->	+0	PC(L)		 -+
	 *	   		+1	PC(M)		  | INT #34 偺栠傝愭
	 *			+2	FLG(L)		  |
	 *			+3	FLG(H)/PC(H) -+
	 *	ISP+4->	+4	PC(L)		 -+
	 *			+5	PC(M)		  | 妱傝崬傒僴儞僪儔偺栠傝愭
	 *			+6	FLG(L)		  |
	 *			+7	FLG(H)/PC(H) -+
	 * ---------------------------------------------------------------------*/
	add.w	#4, sp

	/* 偙傟偑僱僗僩偟偨妱傝崬傒偐丄僨傿僗僷僢僠偑昁梫側偗傟偽儕僞乕儞偡傞丅
	 * ----------------------------------------------------------------------
	 * NOTE:
	 *	嵟弶偺妱傝崬傒偐偳偆偐傪挷傋傞偨傔偵丄reit偱暅妶偝偣傞FLG儗僕僗僞拞偺
	 *	U 僼儔僌傪僥僗僩偡傞丅U=1偱偁傟偽丄儐乕僓僾儘僌儔儉傊偺儕僞乕儞偱偁傝
	 *	嵟弶偺妱傝崬傒偱偁傞丅U=0側傜丄懡廳妱傝崬傒偐傜1儗儀儖愺偄妱傝崬傒傊
	 *	偺儕僞乕儞偱偁傝丄僨傿僗僷僢僠傪峴偭偰偼側傜側偄丅
	 *
	 *	M16C偺応崌丄僗僞僢僋億僀儞僞(sp)憡懳傾僪儗僢僔儞僌偺巊偊傞柦椷偑偁傑
	 *	傝側偄偺偱丄FLG儗僕僗僞埲奜傪夡偝側偄傛偆偵僗僞僢僋忋偺U僼儔僌抣傪
	 *	僥僗僩偡傞捈愙揑側曽朄偑側偄丅桞堦壜擻側偺偼mov柦椷偵傛傞揮憲側偺偱丄
	 *	儕僞乕儞愭偺FLG抣傪枹巊梡偺RAM傊揮憲偡傞偙偲偱丄U僼儔僌(bit7)偺忬懺傪
	 *	N僼儔僌偱抦傞偙偲偑偱偒傞丅mov.b 2[sp], _az_work
	 * ---------------------------------------------------------------------
	 * NOTE:
	 *	捈屻偺儔儀儖 ret_int 偼丄傾僙儞僽儕尵岅偱婰弎偟偨妱傝崬傒僴儞僪儔偺
	 *	嵟屻偵丄reit 偱栠傞戙傢傝偵 jmp ret_int 偡傞偨傔偺傕偺丅傾僙儞僽儕
	 *	尵岅偱彂偐側偗傟偽側傜側偄傛偆側僗僺乕僪傪昁梫偲偡傞妱傝崬傒僴儞僪儔
	 *	偱偼丄INT #34 偵昁梫側19僒僀僋儖昁梫偲側傞僜僼僩僂僃傾妱傝崬傒偺
	 *	僆乕僶乕僿僢僪傪旔偗傞偙偲偑偱偒傞丅
	 * ---------------------------------------------------------------------*/
	.glb	ret_int
ret_int:

	mov.b	2[sp], _az_work
	jpz		ret_now
	cmp.b	#1, _az_dsptsk
	jz		dispatch_now
ret_now:
	reit

dispatch_now:
	/* ----------------------------------------------------------------------
	 * NOTE:
	 *	僨傿僗僷僢僠梫媮偁傝偺妱傝崬傒偐傜僞僗僋傊偺儕僞乕儞偼丄埲壓偺庤弴偱
	 *	峴偆丅偙傟偼丄AzkiRTOS偑儐乕僓僗僞僢僋忋傊僐儞僥僉僗僩(儗僕僗僞)傪
	 *	曐懚偡傞偨傔丄妱傝崬傒僴儞僪儔偐傜僞僗僋傊偺栠傝斣抧傗僼儔僌傪儐乕僓
	 *	僗僞僢僋傊堏摦偟側偗傟偽側傜側偄偨傔丄彮乆暘偐傝擄偔側偭偰偄傞丅
	 *
	 *	USP -->	+00	R0
	 *	仾		+02	R1
	 *	仾		+04	R2
	 *	仾		+06 R3
	 *	pushm	+08	A0
	 *	仾		+10	A1
	 *	仾		+12	FB
	 *	USP -->	+14	PC(L)		  --+		+--	ISP	-->	+00	PC(L)
	 *	仾		+15	PC(M)			|  COPY	| 	 伀		+01	PC(M)
	 *	-4		+16	FLG(L)			| <----	| 	 +4		+02	FLG(L)
	 *	仾		+17	FLG(H)/PC(H)  --+ 		+--	 伀		+03	FLG(H)/PC(H)
	 *	USP -->									ISP -->
	 *  ---------------------------				---------------------------
	 *			儐乕僓僗僞僢僋							妱傝崬傒僗僞僢僋
	 *
	 *	1)	傑偢嵟弶偵丄儐乕僓僗僞僢僋偵4僶僀僩嬻偒傪嶌偭偰偐傜丄
	 *	2)	僞僗僋偺儗僕僗僞傪僞僗僋偺僗僞僢僋偵僾僢僔儏偟丄
	 *	3)	1)偱嶌偭偨嬻4僶僀僩傊妱傝崬傒僗僞僢僋偐傜栠傝斣抧偲僼儔僌傪僐僺乕偡傞丅
	 *	4)	僴乕僪僂僃傾妱傝崬傒偱愊傫偩栠傝斣抧偲僼儔僌傪僉儍儞僙儖偟丄
	 *	5)	僨傿僗僷僢僠傊旘傃崬傓丅
	 *----------------------------------------------------------------------*/
	fset	u
	add.w	#-4, sp
	pushm	r0, r1, r2, r3, a0, a1, fb
	stc		isp, a0
	mov.w	0[a0], 14[sp]
	mov.w	2[a0], 16[sp]
	fclr	u

	add.w	#4, sp

	jsr.w	_az_dispatch
#pragma ENDASM
	/* 偙偙傊偼栠偭偰棃側偄 */
}

void az_dispatch()
{
	/* 傕偭偲傕桪愭弴埵偺崅偄僞僗僋傪RUN忬懺偵偡傞 */
	/* 偙偺帪揰偱僇儗儞僩僞僗僋偺儗僕僗僞偼丄
	   偡傋偰儐乕僓僗僞僢僋忋傊戅旔偟偰偁傞偲壖掕偡傞 */
	AzTCB* tcb;
	int index;

	/* 僨傿僗僷僢僠梫媮僼儔僌傪搢偡 */
	az_cancel_dispatch();

	/* 尰帪揰偱RUN忬懺偺僞僗僋傪READY忬懺傊愗傝懼偊傞 */
	if (tcb = az_curtcb)
	{
		if (tcb->m_state == TTS_RUN)
			tcb->m_state = TTS_RDY;

		/* 儐乕僓僗僞僢僋億僀儞僞傪 AzTCB::m_usp 傊曐懚偡傞 */
		_asm("mov.w $$[fb], a0", tcb);
		_asm("fset  u");
		_asm("stc   sp, 22[a0]");
		_asm("fclr  u");
	}

	for (index = 0; index < AZ_NPRIORITIES; index++)
	{
		if (az_ready[index].m_next != &az_ready[index])
		{
			/* 嵟傕桪愭弴埵偺崅偄僞僗僋敪尒 */
			tcb = MAINLINK_TO_TCB(az_ready[index].m_next);
			tcb->m_state = TTS_RUN;
			az_curtcb = tcb;

			/* 儕僞乕儞偡傞慜偵丄偙偺娭悢偺僗僞僢僋僼儗乕儉傪僉儍儞僙儖偡傞
			   ISP = FB + 5 (媽FB + 媽PC 偺崌寁5僶僀僩) */
			_asm("stc   fb, r0");
			_asm("add.w #5, r0");
			_asm("ldc   r0, isp");

			/* TCB偐傜USP傪庢摼/愝掕偟丄偦傟傪巊偭偰儕僞乕儞偡傞 */
			_asm("mov.w $$[fb], a0", tcb);
			_asm("fset  u");
			_asm("ldc   22[a0], sp");