space.c

T-kernel 的extension源代码

				break; /* Invalid PTE */
			}

			if ( pte[i].c.p == 0 ) {
				/* Page in. */
				err = PageIn(la, lsid);
				if ( err < E_OK ) {
					goto err_ret2;
				}
			} else {
				/* To access a physical address directly,
				   flush the cache. */
				if ( curspc != 0 ) {
					ExtFlushCacheWB(la, 0);
				}
			}

			pa = PFAtoLADR(pte[i].c.pfa);

			if ( alen == 0 ) {
				/* First page */
				n = PAGESIZE - (W)POFS_NUM(la);
				if ( n > len ) {
					n = len;
				}
				memcpy(buf, pa + POFS_NUM(la), (size_t)n);
			} else {
				n = len - alen;
				if ( n > PAGESIZE ) {
					n = PAGESIZE;
				}
				memcpy((VB*)buf + alen, pa, (size_t)n);
			}
			alen += n;
			if ( alen >= len ) {
				break;
			}
		}
	} while ( i == (W)N_PTE );

	/* Reset resident state of buf. */
	err = __UnlockSpace(buf, len);
	if ( err < E_OK ) {
		goto err_ret1;
	}

	return alen;

err_ret2:
	(void)__UnlockSpace(buf, len);
err_ret1:
	DEBUG_PRINT(("__ReadMemSpace err = %d\n", err));
	return err;
}

EXPORT W _ReadMemSpace( VP laddr, VP buf, W len, UW lsid )
{
	W	err;

	LockSEG();
	err = __ReadMemSpace(laddr, buf, len, lsid);
	UnlockSEG();
	return err;
}

/*
 * Write to memory
 *	Write up to len bytes from the logical address buf located in a logical space
 *	of the task to a logical address laddr on lsid logical space.
 *	Write to a range of memory (starting from laddr) regardless of access right and
 *	return the number of bytes as a return value.
 *	It is necessary to prepare readable memory space for len bytes
 *	in buf area.
 *	When called from user level, return error (E_CTX).
 *	Return value	0 - len	Number of written bytes
 *			< 0	Error
 */
EXPORT W __WriteMemSpace( VP laddr, VP buf, W len, UW lsid )
{
	VP	uatb;
	VB	*la, *pa;
	PDE	*pde;
	PTE	*pte;
	PFE	*pfe;
	BOOL	curspc;
	W	i, n, alen;
	UW	imask;
	ER	err;

	if ( len <= 0 ) {
		err = E_PAR;
		goto err_ret1;
	}

	/* Disable calls from user level. */
	if ( (SCInfo.taskmode & (UW)TMF_PPL(TSD_LSC_PPL_3)) == (UW)TMF_PPL(TSD_LSC_PPL_3) ) {
		err = E_CTX;
		goto err_ret1;
	}

	/* Check buf access. */
	err = _ChkSpace(buf, len, MA_READ, SCInfo.taskmode);
	if ( err < E_OK ) {
		goto err_ret1;
	}

	/* Non-logical spaces are accessed directly. */
	if ( !isLogicalSpace(laddr) ) {
		n = _ChkSpaceLen(laddr, len, MA_WRITE, SCInfo.taskmode, lsid);
		if ( n > 0 ) {
			memmove(laddr, buf, (size_t)n);
		}
		return n;
	}

	/* Make buf resident. */
	err = __LockSpace(buf, len);
	if ( err < E_OK ) {
		goto err_ret1;
	}

	/* Performed in the current space: TRUE */
	curspc = ( !isLocalSpace(laddr) || isCurrentSpace(lsid) );

	uatb = GetUATB_lsid(lsid);

	alen = 0;
	do {
		la = (VB*)laddr + alen;

		/* Obtain page directory entry */
		pde = GetPDE(la, uatb, &err);
		if ( pde == NULL ) {
			goto err_ret2;
		}
		if ( pde->c[0].p == 0U ) {
			break; /* No page table */
		}

		/* Obtain page table address. */
		pte = PFAtoLADR(pde->c[0].pfa);

		for ( i = (W)PTBL_NUM(la); i < (W)N_PTE; ++i ) {
			if ( pte[i].w == (UW)PTE_NONE ) {
				break; /* Invalid PTE */
			}

			if ( pte[i].c.p == 0 ) {
				/* Page in. */
				err = PageIn(la, lsid);
				if ( err < E_OK ) {
					goto err_ret2;
				}
			}

			pa = PFAtoLADR(pte[i].c.pfa);
			pfe = LADRtoPFE(pa);
			if ( pte[i].c.w != 0U ) {
				pfe->upd = TRUE; /* Set update flag */
			} else {
				/* If write-protected pages are updated,
				   return error. */
				pfe->err = TRUE;
			}

			/* To access a physical address directly, flush the cache.
			 * Then, disable interrupt to prevent the address from
			 * being cached again by another access.
			 */
			DI(imask);
			if ( curspc != 0 ) {
				ExtFlushCacheWB(la, 0);
			}
			if ( alen == 0 ) {
				/* First page */
				n = PAGESIZE - (W)POFS_NUM(la);
				if ( n > len ) {
					n = len;
				}
				memcpy(pa + POFS_NUM(la), buf, (size_t)n);
			} else {
				n = len - alen;
				if ( n > PAGESIZE ) {
					n = PAGESIZE;
				}
				memcpy(pa, (VB*)buf + alen, (size_t)n);
			}
			ExtFlushCacheWB(pa, 0);
			EI(imask);
			alen += n;
			if ( alen >= len ) {
				break;
			}
		}
	} while ( i == (W)N_PTE );

	/* After writing directly to a physical address, flush the cache. */
	ExtFlushCacheWT();

	/* Reset resident state of buf. */
	err = __UnlockSpace(buf, len);
	if ( err < E_OK ) {
		goto err_ret1;
	}

	return alen;

err_ret2:
	(void)__UnlockSpace(buf, len);
err_ret1:
	DEBUG_PRINT(("__WriteMemSpace err = %d\n", err));
	return err;
}

EXPORT W _WriteMemSpace( VP laddr, VP buf, W len, UW lsid )
{
	W	err;

	LockSEG();
	err = __WriteMemSpace(laddr, buf, len, lsid);
	UnlockSEG();
	return err;
}

/*
 * Set to memory (Byte)
 *	Write up to len bytes of data to a logical address laddr on
 *	lsid logical space.
 *	Write to a range of memory (starting from laddr) regardless of access right and
 *	return the number of bytes as a return value.
 *	When called from user level, return error (E_CTX).
 *	Return value	0 - len	Number of written bytes
 *			< 0	Error
 */
EXPORT W __SetMemSpaceB( VP laddr, W len, UB data, UW lsid )
{
	VP	uatb;
	VB	*la, *pa;
	PDE	*pde;
	PTE	*pte;
	PFE	*pfe;
	BOOL	curspc;
	W	i, n, alen;
	UW	imask;
	ER	err;

	if ( len <= 0 ) {
		err = E_PAR;
		goto err_ret;
	}

	/* Disable calls from user level. */
	if ( (SCInfo.taskmode & (UW)TMF_PPL(TSD_LSC_PPL_3)) == (UW)TMF_PPL(TSD_LSC_PPL_3) ) {
		err = E_CTX;
		goto err_ret;
	}

	/* Non-logical spaces are accessed directly. */
	if ( !isLogicalSpace(laddr) ) {
		n = _ChkSpaceLen(laddr, len, MA_WRITE, SCInfo.taskmode, lsid);
		if ( n > 0 ) {
			memset(laddr, (W)data, (size_t)n);
		}
		return n;
	}

	/* Performed in the current space: TRUE */
	curspc = ( !isLocalSpace(laddr) || isCurrentSpace(lsid) );

	uatb = GetUATB_lsid(lsid);

	alen = 0;
	do {
		la = (VB*)laddr + alen;

		/* Obtain page directory entry */
		pde = GetPDE(la, uatb, &err);
		if ( pde == NULL ) {
			goto err_ret;
		}
		if ( pde->c[0].p == 0U ) {
			break; /* No page table */
		}

		/* Obtain page table address. */
		pte = PFAtoLADR(pde->c[0].pfa);

		for ( i = (W)PTBL_NUM(la); i < (W)N_PTE; ++i ) {
			if ( pte[i].w == (UW)PTE_NONE ) {
				break; /* Invalid PTE */
			}

			if ( pte[i].c.p == 0U ) {
				/* Page in. */
				err = PageIn(la, lsid);
				if ( err < E_OK ) {
					goto err_ret;
				}
			}

			pa = PFAtoLADR(pte[i].c.pfa);
			pfe = LADRtoPFE(pa);
			if ( pte[i].c.w != 0U ) {
				pfe->upd = TRUE; /* Set update flag */
			} else {
				/* If write-protected pages are updated,
				   return error. */
				pfe->err = TRUE;
			}

			/* To access a physical address directly, flush the cache.
			 * Then, disable interrupt to prevent the address from
			 * being cached again by another access.
			 */
			DI(imask);
			if ( curspc != 0 ) {
				ExtFlushCacheWB(la, 0);
			}
			if ( alen == 0 ) {
				/* First page */
				n = PAGESIZE - (W)POFS_NUM(la);
				if ( n > len ) {
					n = len;
				}
				memset(pa + POFS_NUM(la), (W)data, (size_t)n);
			} else {
				n = len - alen;
				if ( n > PAGESIZE ) {
					n = PAGESIZE;
				}
				memset(pa, (W)data, (size_t)n);
			}
			ExtFlushCacheWB(pa, 0);
			EI(imask);
			alen += n;
			if ( alen >= len ) {
				break;
			}
		}
	} while ( i == (W)N_PTE );

	/* After writing directly to physical memory, flush the cache. */
	ExtFlushCacheWT();

	return alen;

err_ret:
	DEBUG_PRINT(("__SetMemSpaceB err = %d\n", err));
	return err;
}

EXPORT W _SetMemSpaceB( VP laddr, W len, UB data, UW lsid )
{
	W	err;

	LockSEG();
	err = __SetMemSpaceB(laddr, len, data, lsid);
	UnlockSEG();
	return err;
}

/* ------------------------------------------------------------------------ */
/*
 * Initialization related to logical space control
 *	(before T-Kernel startup)
 */
EXPORT ER InitLogicalSpace( RealMemoryInfo *meminfo )
{
IMPORT	void	TmpDAbortHdr( void );
	UW	cr;
	W	i, sz;
	ER	err;

	/* Obtain page tables of the system.
	 *	MMU has already been enabled by the monitor.
	 */
	Asm("mrc p15, 0, %0, cr2, c0": "=r"(cr));
	SATB = (PDE*)(cr & TSD_ILS_MSK_0XFFFFC000);

	/* Clear page tables in a virtual memory area managed by the OS. */
	for ( i = LOGICALSPACE_SECTION_TOP;
				i < (W)LOGICALSPACE_SECTION_END; ++i ) {
		SATB->w[i] = PDE_NONE;
	}
#ifdef LOGICALSPACE_SECTION_TOP2
	for ( i = LOGICALSPACE_SECTION_TOP2;
				i < LOGICALSPACE_SECTION_END2; ++i ) {
		SATB->w[i] = PDE_NONE;
	}
#endif
#ifdef LOGICALSPACE_SECTION_TOP3
	for ( i = LOGICALSPACE_SECTION_TOP3;
				i < LOGICALSPACE_SECTION_END3; ++i ) {
		SATB->w[i] = PDE_NONE;
	}
#endif
	PurgeAllTLB();

	/* Maximum number of unique spaces (Max number of processes + 1) */
	err = GetSysConf_bms((UB*)"MaxProc", (W*)&MaxLSID, 1);
	if ( err < E_OK ) {
		goto err_ret;
	}
	MaxLSID++;

	/* Generate unique space page directory */
	sz = (W)MaxLSID * ((W)sizeof(PDE) * NUM_PDIR_ENTRIES / TSD_ILS_DIV_4);
	UATB = AllocRealMem(meminfo, (UW)sz);
	if ( UATB == NULL ) {
		err = E_NOMEM;
		goto err_ret;
	}
	bzero(UATB, (size_t)sz);

#if !RESIDENT_ONLY
	/* Register provisional page fault handler */
	define_inthdr(EIT_DABORT, TmpDAbortHdr);
#endif

	/* Set domain */
	cr = TSD_ILS_CR_0X55555555;
	Asm("mcr p15, 0, %0, cr3, c0":: "r"(cr));

	/* Set modes of MMU and cache.
	 *	Although these modes must have been set by the monitor, make settings anew.
	 */
	Asm("mrc p15, 0, %0, cr1, c0": "=r"(cr));
	cr = (cr & ~(UW)(CR1_R)) | (UW)(CR1_M|CR1_A|CR1_C|CR1_W|CR1_S);
	Asm("mcr p15, 0, %0, cr1, c0":: "r"(cr));

	return E_OK;

err_ret:
	BMS_DEBUG_PRINT(("InitLogicalSpace err = %d\n", err));
	return err;
}

/*
 * Set page table corresponding to the entire real memory area,
 * and obtain real memory area information.
 *	(before T-Kernel startup)
 *
 *	Page tables in the physical memory area have been set by the monitor.
 */
EXPORT ER InitRealMemoryArea( RealMemoryInfo **p_meminfo )
{
IMPORT	VP	lowmem_top, lowmem_limit;	/* Low-level memory management information */
static	RealMemoryInfo	meminfo;
	W	val[L_SYSCONF_VAL];
	UW	memend;
	W	i;

	/* Value limitation on end address of real memory area */
	i = GetSysConf((UB*)"RealMemEnd", val);
	memend = (UW)(( i == 1 )? val[0]: REALMEMORY_END);

	if ( (UW)lowmem_limit < memend ) {
		memend = (UW)lowmem_limit;
	}

	/* Set memory area information */
	meminfo.a[0].b.laddr = lowmem_top;
	meminfo.a[0].b.nbyte = memend - (UW)lowmem_top;
	meminfo.n = 1;

	/* Update free space in memory */
	lowmem_top = (VP)memend;
	*p_meminfo = &meminfo;
	return E_OK;
}

/* ======================================================================== */
/*
 *	Debug support function
 */
#if DEBUGFUNC_SUPPORT

#define	M(v, c)		( ( (v) != 0 )? (c): '-' )

/*
 * Indicate state of logical space
 */
EXPORT void DumpSpace( W pid, VP sadr, VP eadr)
{
	PTH	pth;
	PTE	pte;
	W	npage;
	PINFO	*pinfo;
	UW	lsid;
	ER	err;

	if ( pid > 0 ) {
		err = GetPidToPinfo(pid, &pinfo);
		if ( err < E_OK ) {
			DEBUG_PRINT(("** PID ERROR\n"));
			return;
		}

		lsid = GetLSID_pinfo(pinfo);
	} else {
		lsid = 0;
	}

	if ( !isLogicalSpace(sadr) || !isLogicalSpace(eadr) ) {
		DEBUG_PRINT(("** NO LOGICAL SPACE\n"));
		return;
	}

	sadr = PageAlignL(sadr);
	npage = PageCount((UW)eadr - (UW)sadr);

	DEBUG_PRINT(("[%2d]: LSID:%d  UATB:0x%08x\n",
		pid, lsid, GetUATB_lsid(lsid)));

	InitPTH(&pth, sadr, lsid);
	DumpMore(0);
	while ( --npage >= 0 ) {
		err = NextPTE(&pth);
		if ( err < E_OK ) {
			break;
		}
		pte.w = GetPTE(&pth);
		if ( pte.w == (UW)PTE_NONE ) {
			continue;
		}

		DEBUG_PRINT(("  [%08x] %05x %c%c%c%c%c%c%c\n",
			pth.laddr,		/* logical address */
			pte.c.pfa,		/* page frame address */
			M(pte.c.p,	'P'),	/* present */
			M(pte.c.va,	'V'),	/* valid */
			M(pte.c.clr,	'C'),	/* clear */
			M(pte.c.w,	'W'),	/* writable */
			M(pte.c.u,	'U'),	/* user */
			M(pte.c.c,	'c'),	/* cache */
			M(pte.c.b,	'b')	/* write buffer */
		));
		if ( DumpMore(1) ) {
			break;
		}
	}
	EndPTH(&pth, FALSE);
}

#endif /* DEBUGFUNC_SUPPORT */