excmgr.c

T-kernel 的extension源代码

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

/*
 *	excmgr.c (memory)
 *
 *	System exception handling (ARM Virtual storage version)
 */

#include "segmgr.h"
#include <sys/sysexc.h>
#include <tm/tmonitor.h>
#include <sys/bdm.h>



IMPORT	void	_ExitTask( PINFO *pi, ID tskid );  /* proc/prcmgr.c */

#define	BMS_KILL_PROC	SWI_KILLPROC	/* Process forced termination request from monitor */

/* Message to system exception handling task */
typedef struct {
	BDM_ExcInfo	i;		/* Exception information */
	unsigned int	pgfault:1;	/* Page fault: TRUE */
	unsigned int	killproc:1;	/* Process forced termination request: TRUE */
} ExcMsg;

Inline UW getFSR( void );
Inline void setFSR( UW fsr );
Inline UW getFAR( void );
Inline void setFAR( UW far );
Inline BOOL TaskIndependent( void );

LOCAL void print_regs( ID tid );
LOCAL ER sendExcMsg( ExcMsg *msg );
LOCAL void recvExcMsg( ExcMsg *msg );
LOCAL void SysExcTsk( void );
LOCAL ER RegistPageFaultHdr( void );

/* Message buffer for communicating with system exception handling task */
#define	MaxExcMsg	64U
LOCAL	ExcMsg	ExcMsg_buf[MaxExcMsg];
LOCAL	UW	ExcMsg_top, ExcMsg_cnt;

LOCAL	ID	SysExcTskID;		/* System exception handling task */
EXPORT	FP	DefaultHandlerEntry;	/* Monitor default handler */

#define TSD_HDR_VAL_3		3
#define TSD_PFH_VAL_M1		(-1)
#define TSD_IEM_STK_2048	2048

/*
 * Read/set fault status register (FSR)
 */
Inline UW getFSR( void )
{
	UW	fsr;
	Asm("mrc p15, 0, %0, cr5, c0": "=r"(fsr));
	return fsr;
}
Inline void setFSR( UW fsr )
{
	Asm("mcr p15, 0, %0, cr5, c0":: "r"(fsr));
}

/*
 * Read/set fault address register (FAR)
 */
Inline UW getFAR( void )
{
	UW	far;
	Asm("mrc p15, 0, %0, cr6, c0": "=r"(far));
	return far;
}
Inline void setFAR( UW far )
{
	Asm("mcr p15, 0, %0, cr6, c0":: "r"(far));
}

/*
 * Exception occurred in the task-independent section: TRUE
 */
Inline BOOL TaskIndependent( void )
{
	return ( SCInfo.taskindp > 1 )? TRUE: FALSE;
}

/*
 * Indicate task register value
 */
LOCAL void print_regs( ID tid )
{
#if DEBUGFUNC_SUPPORT
	T_REGS	g;
	T_EIT	e;
	T_CREGS	c;
	ER	er;

	er = tk_get_reg(tid, &g, &e, &c);
	if ( er < E_OK ) {
		BMS_DEBUG_PRINT(("- register can not read -\n"));
		return;
	}

	PrintTaskRegister(bms_printf, &g, &e, &c);
#endif /* DEBUGFUNC_SUPPORT */
}

/*
 * Send exception message
 *	Called in interrupt-disabled state
 */
LOCAL ER sendExcMsg( ExcMsg *msg )
{
	UW	pos;

	if ( ExcMsg_cnt >= (UW)MaxExcMsg ) {
		return E_LIMIT;
	}

	pos = (UW)((ExcMsg_top + ExcMsg_cnt) % (UW)MaxExcMsg);
	ExcMsg_buf[pos] = *msg;

	if ( ExcMsg_cnt++ == 0U ) {
		tk_wup_tsk(SysExcTskID);
	}

	return E_OK;
}

/*
 * Receive exception message
 */
LOCAL void recvExcMsg( ExcMsg *msg )
{
	UINT	imask;

	for ( ;; ) {
		DI(imask);
		if ( ExcMsg_cnt > 0U ) {
			*msg = ExcMsg_buf[ExcMsg_top];
			if ( ++ExcMsg_top >= (UW)MaxExcMsg ) {
				ExcMsg_top = 0U;
			}
			ExcMsg_cnt--;
			break;
		}
		EI(imask);

		tk_slp_tsk(TMO_FEVR);
	}
	EI(imask);
}

/*
 * System exception handling task
 */
LOCAL void SysExcTsk( void )
{
IMPORT	WER	_snd_hdr_msg( MESSAGE *msg, W opt );	/* Message management */
	ExcMsg	excmsg;
	PINFO	*pinfo;
	ID	oldrid;
	EXCMESG	msg;
	ER	er;

	for ( ;; ) {	/* Not terminate on its own */

		/* Receive system exception */
		recvExcMsg(&excmsg);

		if ( excmsg.pgfault != 0U ) {
			/* Handle page fault */
			UW lsid = GetLSID_tid(excmsg.i.taskid);
			LockSEG();
			er = PageIn((VP)excmsg.i.excaddr, lsid);
			UnlockSEG();
			if ( er >= E_OK ) {
				/* Reopen intended task */
				er = tk_frsm_tsk(excmsg.i.taskid);
				if ( er >= E_OK ) {
					continue;
				}
			}
			DEBUG_PRINT(("SysExcTsk PageIn err = %d\n", er));
		}

		/* Obtain PINFO on exception occurrence process. */
		pinfo = GetPinfo(excmsg.i.taskid);
		if ( pinfo == NULL ) {
			goto system_down;
		}

		/* If the task is in the course of termination processing,
		   call task termination processing directly from process management. */
		if ( pinfo->exitkind != 0 ) {
			oldrid = tk_set_rid(TSK_SELF, pinfo->resid);
			_ExitTask(pinfo, excmsg.i.taskid);
			tk_set_rid(TSK_SELF, oldrid);
			continue;
		}

		if ( excmsg.killproc != 0U ) {
			goto kill_proc;
		}

		/* Send message */
		msg.type = MS_SYS1;
		msg.size = (W)(sizeof(EXCMESG) - offsetof(EXCMESG, taskid));
		msg.taskid = excmsg.i.taskid;
		msg.procid = pinfo->procid;
		msg.vecno   = excmsg.i.vecno;
		msg.excinfo = excmsg.i.excinfo;
		msg.excaddr = excmsg.i.excaddr;

		/* Send to exception server */
		er = _snd_hdr_msg((MESSAGE*)&msg, NOWAIT);
		if ( er <= 0 ) {
			DEBUG_PRINT(("SysExcTsk _snd_hdr_msg err = %d\n", er));

kill_proc:
			/* If message could not be sent to exception server,
			   forcedly terminate the process. */
			tk_ras_tex(excmsg.i.taskid, 0);
			if ( excmsg.i.taskid != pinfo->tsk[0].tskid ) {
				/* Terminate main task as well. */
				tk_ras_tex(pinfo->tsk[0].tskid, 0);
			}
			pinfo->exitkind = MS_ABORT;
			tk_frsm_tsk(excmsg.i.taskid);
		}
	}

system_down:
	/* Use monitor to output message */
	BMS_DEBUG_PRINT(("\n***** OS SYSTEM DOWN *****\n"));
	BMS_DEBUG_PRINT(("INT:%d FSR:%08x FAR:%08x tid:%d\n",
		excmsg.i.vecno, excmsg.i.excinfo, excmsg.i.excaddr,
		excmsg.i.taskid));
	print_regs(excmsg.i.taskid);
	tm_monitor();	/* To monitor */
}

/*
 * System exception handler
 */
EXPORT void SysExcHdr( W vecno, ExcStack *sp )
{
	ExcMsg	excmsg;
	PINFO	*pinfo;
	ER	er;

	memset(&excmsg, 0, (size_t)sizeof(excmsg));

	if ( vecno == -BMS_KILL_PROC ) {
		vecno = -vecno;
		excmsg.killproc = TRUE;
	}

	excmsg.i.vecno   = (UW)vecno;
	excmsg.i.excinfo = getFSR();
	excmsg.i.excaddr = getFAR();
	excmsg.pgfault = FALSE;

	EI(0);	/* Multiplex interrupt enabled */

	/* Obtain intended task ID */
	excmsg.i.taskid = tk_get_tid();
	if ( excmsg.i.taskid <= 0 ) {
		goto system_down;
	}

	/* If an exception occurs during the execution of interrupt handler (task independent
	   section) or in the interrupt-disabled state, shut down the system. */
	if ( ((sp->spsr & ((UW)PSR_I|(UW)PSR_F)) != 0U)
	  || TaskIndependent() ) {
		goto system_down;
	}

	/* Notify BDM of the exception. */
	er = BDM_NotifyException(&excmsg.i);
	if ( er == E_OK ) {
		return;
	}
	/* If BDM does not exist, continue exception handling. */

	/* If an exception occurs during the execution of a task (extension, SVC, non-process,
	   etc.) within OS, shut down the system. */
	if ( ((SCInfo.taskmode & (UW)TMF_PPL(TSD_HDR_VAL_3)) == (UW)TMF_PPL(0))
	  || ((pinfo = GetPinfo(excmsg.i.taskid)) == NULL) ) {
		goto system_down;
	}

	/* If the current task is a system process that is in the course of termination
	   processing, shut down the system. */
	if ( (pinfo->exitkind != 0)
	  && ((SCInfo.taskmode & (UW)TMF_PPL(TSD_HDR_VAL_3)) < (UW)TMF_PPL(TSD_HDR_VAL_3)) ) {
		goto system_down;
	}

	/* Suspend current task */
	er = tk_sus_tsk(excmsg.i.taskid);
	if ( er < E_OK ) {
		goto system_down;
	}

	/* Notify to exception handling task */
	er = sendExcMsg(&excmsg);
	if ( er < E_OK ) {
		goto system_down;
	}

	return;

	/** Use monitor to indicate message **/
system_down:
	BMS_DEBUG_PRINT(("\n***** OS SYSTEM DOWN *****\n"));
	BMS_DEBUG_PRINT(("INT:%d PC:%08x CPSR:%08x FSR:%08x FAR:%08x tid:%d\n",
		vecno, sp->lr, sp->spsr,
		excmsg.i.excinfo, excmsg.i.excaddr, excmsg.i.taskid));
	tm_monitor();	/* To monitor */
}

/*
 * Page fault handler
 *	EIT_IABORT	Prefetch abort
 *	EIT_DABORT	Data abort
 */
EXPORT W PageFaultHdr( UW vecno, ExcStack *sp )
{
IMPORT	W	DebugMonitorFlag;	/* debug.c */
	UW	excinfo, excaddr;
	PTH	pth;
	PTE	pte;
	ER	er;

	DO_DEBUG( pte.w = TSD_PFH_VAL_M1; )

	if ( vecno == EIT_DABORT ) {
		excinfo = getFSR();
		excaddr = getFAR();
	} else {
		excinfo = 0;
		excaddr = sp->lr;
	}

	EI(0);	/* Multiplex interrupt enabled */

	InitPTH(&pth, (VP)excaddr, 0);
	er = NextPTE(&pth);
	if ( er < E_OK ) {
		goto exception;		/* Invalid access */
	}
	pte.w = GetPTE(&pth);
	if ( pte.w == PTE_NONE ) {
		goto exception;	/* Invalid access */
	}

	if ( vecno == EIT_IABORT ) {
		/* Set exception information for prefetch abort */
		if ( (pte.c.u == 0)
		  && ((SCInfo.taskmode & (UW)TMF_PPL(TSD_HDR_VAL_3)) == (UW)TMF_PPL(TSD_HDR_VAL_3)) ) {
			excinfo = FSR_Permission | FSR_Page;
		} else {
			excinfo = FSR_Translation | FSR_Page;
		}
	}

	/* Except for page absent exception, handle all exceptions as invalid access. */
	if ( (excinfo & (UW)FSR_TypeMask) != (UW)FSR_Translation ) {
		goto exception;
	}

	if ( (pte.c.p == 0) && (pte.c.va != 0) && (SegLock.cnt < 0) ) {
		/* Since the page is valid, make page frame settings only.
		 * However, if LockSEG() cannot be handled here,
		 * pass it to an exception handling task.
		 */
		PFE *pfe = PFAtoPFE(pte.c.pfa);
		MovePageFrame(pfe, PFS_use);
		(void)ChgPTE(&pth, PT_Present, ~PT_Present, TRUE);
	} else {
		ExcMsg	excmsg;
		ID	tid;

		/* Obtain intended task ID */
		tid = er = tk_get_tid();
		if ( er < E_OK ) {
			goto exception;
		}

		memset(&excmsg, 0, (size_t)sizeof(excmsg));
		excmsg.i.taskid  = tid;
		excmsg.i.vecno   = vecno;
		excmsg.i.excinfo = excinfo;
		excmsg.i.excaddr = excaddr;
		excmsg.pgfault = TRUE;

		/* Handle it as exception if it occurred while a task independent section is executed or interrupt is disabled. */
		if ( ((sp->spsr & ((UW)PSR_I|(UW)PSR_F)) != 0U)
		  || TaskIndependent() ) {
			goto exception;
		}

		/* Suspend current task */
		er = tk_sus_tsk(tid);
		if ( er < E_OK ) {
			goto exception;
		}

		/* Notify to exception handling task */
		er = sendExcMsg(&excmsg);
		if ( er < E_OK ) {
			BMS_DEBUG_PRINT(("\nPageFaultHdr sendExcMsg er = %d\n", er));
			goto exception;
		}
	}

	EndPTH(&pth, FALSE);
	return 0;

exception:
	/* Exception not handled by segment management */
	EndPTH(&pth, FALSE);
	BMS_DEBUG_PRINT(("PageFaultHdr vecno=%d excinfo=0x%08x excaddr=0x%08x "
				"pte=0x%08x er=%d\n",
				vecno, excinfo, excaddr, pte.w, er));

	disint();
	setFSR(excinfo);
	setFAR(excaddr);
	if ( DebugMonitorFlag != 0 ) {
		return 1;  /* To BMS */
	}
	SysExcHdr((W)vecno, sp);
	return 0;
}

/*
 * Register/deregister system exception handler
 *	regist = TRUE	Register
 *	regist = FALSE	Deregister
 */
EXPORT void RegistSysExcHdr( BOOL regist )
{
	T_DINT	dint;

	dint.intatr = TA_HLNG;
	dint.inthdr = SysExcHdr;

	tk_def_int(EIT_DEFAULT, ( regist != 0 )? &dint: NULL);
}

/*
 * Register page fault handler
 */
LOCAL ER RegistPageFaultHdr( void )
{
#if !RESIDENT_ONLY
IMPORT	void	asmIAbortHdr( void );
IMPORT	void	asmDAbortHdr( void );
	T_DINT	dint;
	ER	er;

	/* Save monitor default handler. */
	DefaultHandlerEntry = (FP)SCArea->intvec[EIT_DEFAULT];

	/* Register abort handler. */
	dint.intatr = TA_ASM;
	dint.inthdr = asmIAbortHdr;
	er = tk_def_int(EIT_IABORT, &dint);
	if ( er < E_OK ) {
		goto err_ret;
	}
	dint.inthdr = asmDAbortHdr;
	er = tk_def_int(EIT_DABORT, &dint);
	if ( er < E_OK ) {
		goto err_ret;
	}

	return E_OK;

err_ret:
	BMS_DEBUG_PRINT(("RegistPageFaultHdr er = %d\n", er));
	return er;
#else
	return E_OK;
#endif
}

/* ------------------------------------------------------------------------ */

/*
 * Initialize system exception processing
 *	(immediately after T-Kernel startup)
 */
EXPORT ER InitExcMgr( BOOL StartUp )
{
IMPORT	void	asmKillProcHdr( void );

	T_CTSK	ctsk;
	T_DINT	dint;
	ER	er;

	if ( !StartUp ) {
		/* Termination processing: deregister at least the exception handler. */
		RegistSysExcHdr(FALSE);
		return E_OK;
	}

	/* Start system exception processing task */
	SetOBJNAME(ctsk.exinf, "SExc");
	ctsk.tskatr  = TA_HLNG|TA_RNG0;
	ctsk.task    = SysExcTsk;
	ctsk.itskpri = 1;
	ctsk.stksz   = TSD_IEM_STK_2048;
	er = tk_cre_tsk(&ctsk);
	if ( er < E_OK ) {
		goto err_ret;
	}
	er = tk_sta_tsk(SysExcTskID = er, 0);
	if ( er < E_OK ) {
		goto err_ret;
	}

	/* Register page fault handler */
	er = RegistPageFaultHdr();
	if ( er < E_OK ) {
		goto err_ret;
	}

	/* Register BMS_KILL_PROC handler */
	dint.intatr = TA_ASM;
	dint.inthdr = asmKillProcHdr;
	er = tk_def_int(BMS_KILL_PROC, &dint);
	if ( er < E_OK ) {
		goto err_ret;
	}

	return E_OK;

err_ret:
	BMS_DEBUG_PRINT(("InitExcMgr: er = %d\n", er));
	return er;
}