memmap.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.
 *
 *----------------------------------------------------------------------
 */

/*
 *	memmap.c (memory)
 *
 *	Segment management (Virtual storage version)
 *	Device I/O memory map
 */

#include "segmgr.h"
#include <bitop.h>

LOCAL VP allocIOspace( W np );
LOCAL W freeIOspace( VP laddr );

/*
 * Number of I/O space pages
 */
#define	NP_IOSPC	( (IOSPACE_END - IOSPACE_TOP) / PAGESIZE )

#define TSD_IMM_VAL_7		7
#define TSD_IMM_VAL_8		8
#define TSD_MMY_MSK_0X03C	0x03cU

/*
 * Management information on device I/O memory space
 *	Use one bit to indicate "In use (1)/Not used (0)" for each page.
 *	To separate areas requested by MapMemory(), each area is followed
 *	by an unused page.
 *	(*) In addition, these unused pages prevent invalid access that extends beyond the area.
 *
 *	    IOSPACE_TOP	+-----------------------+
 *			| Not used 1Page	|
 *			+-----------------------+
 *			| MapMem #1		|
 *			+-----------------------+
 *			| Not used 1Page	|
 *			+-----------------------+
 *			| MapMem #2		|
 *			+-----------------------+
 *			| Not used		|
 *			:			:
 */
LOCAL UB	*iospc;

/*
 * Obtain I/O space.
 */
LOCAL VP allocIOspace( W np )
{
	W	ofs, i;

	/* Search for free space. */
	ofs = 0;
	while ( ofs < (W)NP_IOSPC ) {
		i = BitSearch0(iospc, ofs, (W)NP_IOSPC - ofs);
		if ( i < 0 ) {
			break; /* No space was found. */
		}

		ofs += i + 1;
		i = BitSearch1(iospc, ofs, (W)NP_IOSPC - ofs);
		if ( i < 0 ) {
			i = (W)NP_IOSPC - ofs;
		}

		if ( i > np ) {
			/* Space was found: set to "In use". */
			BitsSet(iospc, ofs, np);
			return (VP)(IOSPACE_TOP + (UW)(ofs * PAGESIZE));
		}
		ofs += i;
	}

	return NULL;  /* No free area. */
}

/*
 * Release I/O space.
 */
LOCAL W freeIOspace( VP laddr )
{
	W	ofs, np;

	ofs = (W)(((UW)laddr - IOSPACE_TOP) / PAGESIZE);

	if ( (ofs <= 0) || (ofs >= (W)NP_IOSPC) ) {
		return 0;	/* Invalid */
	}
	if ( BitTest(iospc, (UW)(ofs - 1)) != 0 ) {
		return 0;	/* Invalid */
	}

	/* Determine the size of area */
	np = BitSearch0(iospc, ofs, (W)NP_IOSPC - ofs);
	if ( np <= 0 ) {
		return 0;  /* Invalid */
	}

	/* Set to "Not used" */
	BitsClr(iospc, ofs, np);

	return np;
}

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

LOCAL ER __UnmapMemory( VP laddr );

/*
 * Memory map
 *	Map len bytes of area (starting from physical address paddr) to I/O space
 *	and return its logical address as *laddr.
 *	When set at "paddr = NULL", automatically allocate memory that has
 *	consecutive physical addresses and map this area to I/O space.
 *	Logical address cannot be specified, but allocated automatically.
 *	Mapped logical space has an attribute specified by attr.
 *
 *	(*) Note that page fault may occur when access is made to *laddr.
 */
EXPORT ER _MapMemory( VP paddr, W len, UW attr, VP *laddr )
{
	VP	la, mapladdr;
	PDE	*pde;
	PTE	*pte, set;
	PFE	*pfe;
	W	i, np;
	ER	err;

	LockSEG();

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

	/* Allocate logical space */
	i = (W)POFS_NUM(paddr);
	np = (W)PageCount((UW)(len + i));
	la = allocIOspace(np);
	if ( la == NULL ) {
		err = E_LIMIT;
		goto err_ret1;
	}
	mapladdr = (VB*)la + i;

	if ( paddr == NULL ) {
		/* Reserve memory with consecutive addresses */
		paddr = AllocPhysicalMem((UW)np);
		if ( paddr == NULL ) {
			err = E_NOMEM;
			goto err_ret2;
		}
	}

	/* Page table entry setting value */
	set.w = (UW)PageAlignL(paddr) |
		((attr & TSD_MMY_MSK_0X03C) ^ (UW)(PT_Present|PT_Cachable|PT_Bufferable));
	SetPTE_AP(&set, set.a.ap0);

	while ( np > 0 ) {
		/* Obtain page directory entry */
		pde = SATB + PDIR_NUM(la);

		/* If there is no page table, obtain a new one. */
		if ( pde->c[0].p == 0 ) {
			/* Obtain page table */
			pfe = GetPFM(PFS_lock, FALSE);
			if ( pfe == NULL ) {
				err = E_NOMEM;
				goto err_ret3;
			}
			pte = PFEtoLADR(pfe);

			/* Initialize page table */
			memset_w(pte, PTE_NONE, (size_t)N_PTE);

			/* Set page directory */
			SetPDE(pde, (UW)PDE_NORM | (UW)toPhysicalAddress(pte));
			WriteBackPageTable(pde);
		} else {
			/* Obtain page table address and page number */
			pte = PFAtoLADR(pde->c[0].pfa);
			pfe = LADRtoPFE(pte);
		}

		/* Set page table */
		for ( i = (W)PTBL_NUM(la); i < (W)N_PTE; ++i ) {
			pte[i] = set;
			PurgePageTLB(la, &pte[i]);

			pfe->dbn_no++;		/* Number of valid pages */
			la = NextPage(la);	/* Next page */
			set.c.pfa++;
			if ( --np <= 0 ) {
				break;
			}
		}
	}

	UnlockSEG();

	*laddr = mapladdr;
	return E_OK;

err_ret3:
	(void)__UnmapMemory(mapladdr);
	pfe = PFAtoPFE(set.c.pfa);
	if ( pfe != NULL ) {
		while ( np-- > 0 ) {
			DiscardPageFrame(pfe++);
		}
	}
	goto err_ret1;
err_ret2:
	freeIOspace(mapladdr);
err_ret1:
	UnlockSEG();
	DEBUG_PRINT(("_MapMemory err = %d\n", err));
	return err;
}

/*
 * Memory unmapping
 *	Release I/O space allocated by MapMemory().
 *	For laddr, specify a logical address obtained by MapMemory().
 */
LOCAL ER __UnmapMemory( VP laddr )
{
	PDE	*pde;
	PTE	*pte;
	PFE	*pfe;
	W	np, i;
	ER	err, error = E_OK;

	/* Release logical space */
	np = freeIOspace(laddr);
	if ( np <= 0 ) {
		error = E_PAR;
		goto err_ret;
	}

	while ( np > 0 ) {
		/* Obtain page directory entry */
		pde = SATB + PDIR_NUM(laddr);
		if ( pde->c[0].p == 0 ) {
			/* No page table */
			i = (W)(N_PTE - PTBL_NUM(laddr));
			laddr = (VP)((VB*)laddr + (PAGESIZE * i));
			np -= i;
			continue;
		}

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

		for ( i = (W)PTBL_NUM(laddr); i < (W)N_PTE; ++i ) {
			PTE old;
			old = pte[i];

			if ( old.w != PTE_NONE ) {
				if ( old.c.c != 0 ) {
					/* Flush cache */
					ExtFlushCacheWB(laddr, 0);
				}

				/* Set invalid PTE */
				pte[i].w = PTE_NONE;
				PurgePageTLB(laddr, &pte[i]);

				/* Discontinue relations with PTE. */
				err = UnlinkPage(old, NULL);
				if ( err < E_OK ) {
					error = err;
				}

				pfe->dbn_no--;		/* Number of valid pages */
			}

			laddr = NextPage(laddr);	/* Next page */
			if ( --np <= 0 ) {
				break;
			}
		}

		if ( pfe->dbn_no == 0 ) {
			/* When a page table becomes empty,
			   delete the page table itself. */
			SetPDE(pde, PDE_NONE);
			WriteBackPageTable(pde);
			DiscardPageFrame(pfe);
		}
	}
	ExtFlushCacheWT();

	if ( error < E_OK ) {
		goto err_ret;
	}
	return E_OK;

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

EXPORT ER _UnmapMemory( VP laddr )
{
	ER	err;

	LockSEG();
	err = __UnmapMemory(laddr);
	UnlockSEG();

	return err;
}

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

/*
 * Initialization related to device I/O memory map
 */
EXPORT ER InitMemoryMap( void )
{
	ER	err;

	/* Initialize I/O space management table */
	iospc = Icalloc((size_t)1, (size_t)((NP_IOSPC + TSD_IMM_VAL_7) / TSD_IMM_VAL_8));
	if ( iospc == NULL ) {
		err = E_NOMEM;
		goto err_ret;
	}

	return E_OK;

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