/*	$OpenBSD: pmap.c,v 1.160 2015/07/20 00:16:16 kettenis Exp $ */

/*
 * Copyright (c) 2015 Martin Pieuchot
 * Copyright (c) 2001, 2002, 2007 Dale Rahn.
 * All rights reserved.
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Effort sponsored in part by the Defense Advanced Research Projects
 * Agency (DARPA) and Air Force Research Laboratory, Air Force
 * Materiel Command, USAF, under agreement number F30602-01-2-0537.
 */

/*
 * powerpc lazy icache managment.
 * The icache does not snoop dcache accesses. The icache also will not load
 * modified data from the dcache, but the unmodified data in ram.
 * Before the icache is loaded, the dcache must be synced to ram to prevent
 * the icache from loading stale data.
 * pg->pg_flags PG_PMAP_EXE bit is used to track if the dcache is clean
 * and the icache may have valid data in it.
 * if the PG_PMAP_EXE bit is set (and the page is not currently RWX)
 * the icache will only have valid code in it. If the bit is clear
 * memory may not match the dcache contents or the icache may contain
 * data from a previous page.
 *
 * pmap enter
 * !E  NONE 	-> R	no action
 * !E  NONE|R 	-> RW	no action
 * !E  NONE|R 	-> RX	flush dcache, inval icache (that page only), set E
 * !E  NONE|R 	-> RWX	flush dcache, inval icache (that page only), set E
 * !E  NONE|RW 	-> RWX	flush dcache, inval icache (that page only), set E
 *  E  NONE 	-> R	no action
 *  E  NONE|R 	-> RW	clear PG_PMAP_EXE bit
 *  E  NONE|R 	-> RX	no action
 *  E  NONE|R 	-> RWX	no action
 *  E  NONE|RW 	-> RWX	-invalid source state
 *
 * pamp_protect
 *  E RW -> R	- invalid source state
 * !E RW -> R	- no action
 *  * RX -> R	- no action
 *  * RWX -> R	- sync dcache, inval icache
 *  * RWX -> RW	- clear PG_PMAP_EXE
 *  * RWX -> RX	- sync dcache, inval icache
 *  * * -> NONE	- no action
 * 
 * pmap_page_protect (called with arg PROT_NONE if page is to be reused)
 *  * RW -> R	- as pmap_protect
 *  * RX -> R	- as pmap_protect
 *  * RWX -> R	- as pmap_protect
 *  * RWX -> RW	- as pmap_protect
 *  * RWX -> RX	- as pmap_protect
 *  * * -> NONE - clear PG_PMAP_EXE
 * 
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/proc.h>
#include <sys/queue.h>
#include <sys/pool.h>
#include <sys/atomic.h>

#include <uvm/uvm_extern.h>

#include <machine/pcb.h>
#include <powerpc/powerpc.h>
#include <powerpc/bat.h>
#include <machine/pmap.h>

struct bat battable[16];

struct dumpmem dumpmem[VM_PHYSSEG_MAX];
u_int ndumpmem;

struct pmap kernel_pmap_;
static struct mem_region *pmap_mem, *pmap_avail;
struct mem_region pmap_allocated[10];
int pmap_cnt_avail;
int pmap_cnt_allocated;

struct pte_64  *pmap_ptable64;
struct pte_32  *pmap_ptable32;
int	pmap_ptab_cnt;
u_int	pmap_ptab_mask;

#define HTABSIZE_32	(pmap_ptab_cnt * 64)
#define HTABMEMSZ_64	(pmap_ptab_cnt * 8 * sizeof(struct pte_64))
#define HTABSIZE_64	(ffs(pmap_ptab_cnt) - 12)

static u_int usedsr[NPMAPS / sizeof(u_int) / 8];

struct pte_desc {
	/* Linked list of phys -> virt entries */
	LIST_ENTRY(pte_desc) pted_pv_list;
	union {
		struct pte_32 pted_pte32;
		struct pte_64 pted_pte64;
	} p;
	pmap_t pted_pmap;
	vaddr_t pted_va;
};

void pmap_attr_save(paddr_t pa, u_int32_t bits);
void pmap_pted_ro(struct pte_desc *, vm_prot_t);
void pmap_pted_ro64(struct pte_desc *, vm_prot_t);
void pmap_pted_ro32(struct pte_desc *, vm_prot_t);

/*
 * Some functions are called in real mode and cannot be profiled.
 */
#define __noprof __attribute__((__no_instrument_function__))

/* VP routines */
int pmap_vp_enter(pmap_t pm, vaddr_t va, struct pte_desc *pted, int flags);
struct pte_desc *pmap_vp_remove(pmap_t pm, vaddr_t va);
void pmap_vp_destroy(pmap_t pm);
struct pte_desc *pmap_vp_lookup(pmap_t pm, vaddr_t va) __noprof;

/* PV routines */
void pmap_enter_pv(struct pte_desc *pted, struct vm_page *);
void pmap_remove_pv(struct pte_desc *pted);


/* pte hash table routines */
static inline void *pmap_ptedinhash(struct pte_desc *);
void pte_insert32(struct pte_desc *) __noprof;
void pte_insert64(struct pte_desc *) __noprof;
void pmap_fill_pte64(pmap_t, vaddr_t, paddr_t, struct pte_desc *, vm_prot_t,
    int) __noprof;
void pmap_fill_pte32(pmap_t, vaddr_t, paddr_t, struct pte_desc *, vm_prot_t,
    int) __noprof;

void pmap_syncicache_user_virt(pmap_t pm, vaddr_t va);

void _pmap_kenter_pa(vaddr_t va, paddr_t pa, vm_prot_t prot, int flags,
    int cache);
void pmap_remove_pted(pmap_t, struct pte_desc *);

/* setup/initialization functions */
void pmap_avail_setup(void);
void pmap_avail_fixup(void);
void pmap_remove_avail(paddr_t base, paddr_t end);
void *pmap_steal_avail(size_t size, int align);

/* asm interface */
int pte_spill_r(u_int32_t, u_int32_t, u_int32_t, int) __noprof;
int pte_spill_v(pmap_t, u_int32_t, u_int32_t, int) __noprof;

u_int32_t pmap_setusr(pmap_t pm, vaddr_t va);
void pmap_popusr(u_int32_t oldsr);

/* pte invalidation */
void pte_del(void *, vaddr_t);
void pte_zap(void *, struct pte_desc *);

/* XXX - panic on pool get failures? */
struct pool pmap_pmap_pool;
struct pool pmap_vp_pool;
struct pool pmap_pted_pool;

int pmap_initialized = 0;
int physmem;
int physmaxaddr;

#ifdef MULTIPROCESSOR
struct __mp_lock pmap_hash_lock;

#define	PMAP_HASH_LOCK_INIT()		__mp_lock_init(&pmap_hash_lock)

#define	PMAP_HASH_LOCK(s)						\
do {									\
	s = ppc_intr_disable();						\
	__mp_lock(&pmap_hash_lock);					\
} while (0)

#define	PMAP_HASH_UNLOCK(s)						\
do {									\
	__mp_unlock(&pmap_hash_lock);					\
	ppc_intr_enable(s);						\
} while (0)

#define	PMAP_VP_LOCK_INIT(pm)		mtx_init(&pm->pm_mtx, IPL_VM)

#define	PMAP_VP_LOCK(pm)						\
do {									\
	if (pm != pmap_kernel())					\
		mtx_enter(&pm->pm_mtx);					\
} while (0)

#define	PMAP_VP_UNLOCK(pm)						\
do {									\
	if (pm != pmap_kernel())					\
		mtx_leave(&pm->pm_mtx);					\
} while (0)

#define PMAP_VP_ASSERT_LOCKED(pm)					\
do {									\
	if (pm != pmap_kernel())					\
		MUTEX_ASSERT_LOCKED(&pm->pm_mtx);			\
} while (0)

#else /* ! MULTIPROCESSOR */

#define	PMAP_HASH_LOCK_INIT()		/* nothing */
#define	PMAP_HASH_LOCK(s)		(void)s
#define	PMAP_HASH_UNLOCK(s)		/* nothing */

#define	PMAP_VP_LOCK_INIT(pm)		/* nothing */
#define	PMAP_VP_LOCK(pm)		/* nothing */
#define	PMAP_VP_UNLOCK(pm)		/* nothing */
#define	PMAP_VP_ASSERT_LOCKED(pm)	/* nothing */
#endif /* MULTIPROCESSOR */

/* virtual to physical helpers */
static inline int
VP_SR(vaddr_t va)
{
	return (va >>VP_SR_POS) & VP_SR_MASK;
}

static inline int
VP_IDX1(vaddr_t va)
{
	return (va >> VP_IDX1_POS) & VP_IDX1_MASK;
}

static inline int
VP_IDX2(vaddr_t va)
{
	return (va >> VP_IDX2_POS) & VP_IDX2_MASK;
}

#if VP_IDX1_SIZE != VP_IDX2_SIZE 
#error pmap allocation code expects IDX1 and IDX2 size to be same
#endif
struct pmapvp {
	void *vp[VP_IDX1_SIZE];
};


/*
 * VP routines, virtual to physical translation information.
 * These data structures are based off of the pmap, per process.
 */

/*
 * This is used for pmap_kernel() mappings, they are not to be removed
 * from the vp table because they were statically initialized at the
 * initial pmap initialization. This is so that memory allocation 
 * is not necessary in the pmap_kernel() mappings.
 * Otherwise bad race conditions can appear.
 */
struct pte_desc *
pmap_vp_lookup(pmap_t pm, vaddr_t va)
{
	struct pmapvp *vp1;
	struct pmapvp *vp2;
	struct pte_desc *pted;

	PMAP_VP_ASSERT_LOCKED(pm);

	vp1 = pm->pm_vp[VP_SR(va)];
	if (vp1 == NULL) {
		return NULL;
	}

	vp2 = vp1->vp[VP_IDX1(va)];
	if (vp2 == NULL) {
		return NULL;
	}

	pted = vp2->vp[VP_IDX2(va)];

	return pted;
}

/*
 * Remove, and return, pted at specified address, NULL if not present
 */
struct pte_desc *
pmap_vp_remove(pmap_t pm, vaddr_t va)
{
	struct pmapvp *vp1;
	struct pmapvp *vp2;
	struct pte_desc *pted;

	PMAP_VP_ASSERT_LOCKED(pm);

	vp1 = pm->pm_vp[VP_SR(va)];
	if (vp1 == NULL) {
		return NULL;
	}

	vp2 = vp1->vp[VP_IDX1(va)];
	if (vp2 == NULL) {
		return NULL;
	}

	pted = vp2->vp[VP_IDX2(va)];
	vp2->vp[VP_IDX2(va)] = NULL;

	return pted;
}

/*
 * Create a V -> P mapping for the given pmap and virtual address
 * with reference to the pte descriptor that is used to map the page.
 * This code should track allocations of vp table allocations
 * so they can be freed efficiently.
 */
int
pmap_vp_enter(pmap_t pm, vaddr_t va, struct pte_desc *pted, int flags)
{
	struct pmapvp *vp1;
	struct pmapvp *vp2;

	PMAP_VP_ASSERT_LOCKED(pm);

	vp1 = pm->pm_vp[VP_SR(va)];
	if (vp1 == NULL) {
		vp1 = pool_get(&pmap_vp_pool, PR_NOWAIT | PR_ZERO);
		if (vp1 == NULL) {
			if ((flags & PMAP_CANFAIL) == 0)
				panic("pmap_vp_enter: failed to allocate vp1");
			return ENOMEM;
		}
		pm->pm_vp[VP_SR(va)] = vp1;
	}

	vp2 = vp1->vp[VP_IDX1(va)];
	if (vp2 == NULL) {
		vp2 = pool_get(&pmap_vp_pool, PR_NOWAIT | PR_ZERO);
		if (vp2 == NULL) {
			if ((flags & PMAP_CANFAIL) == 0)
				panic("pmap_vp_enter: failed to allocate vp2");
			return ENOMEM;
		}
		vp1->vp[VP_IDX1(va)] = vp2;
	}

	vp2->vp[VP_IDX2(va)] = pted;

	return 0;
}

static inline void
tlbie(vaddr_t va)
{
	asm volatile ("tlbie %0" :: "r"(va & ~PAGE_MASK));
}

static inline void
tlbsync(void)
{
	asm volatile ("tlbsync");
}
static inline void
eieio(void)
{
	asm volatile ("eieio");
}

static inline void
sync(void)
{
	asm volatile ("sync");
}

static inline void
tlbia(void)
{
	vaddr_t va;

	sync();
	for (va = 0; va < 0x00040000; va += 0x00001000)
		tlbie(va);
	eieio();
	tlbsync();
	sync();
}

static inline int
ptesr(sr_t *sr, vaddr_t va)
{
	return sr[(u_int)va >> ADDR_SR_SHIFT];
}

static inline int 
pteidx(sr_t sr, vaddr_t va)
{
	int hash;
	hash = (sr & SR_VSID) ^ (((u_int)va & ADDR_PIDX) >> ADDR_PIDX_SHIFT);
	return hash & pmap_ptab_mask;
}

#define PTED_VA_PTEGIDX_M	0x07
#define PTED_VA_HID_M		0x08
#define PTED_VA_MANAGED_M	0x10
#define PTED_VA_WIRED_M		0x20
#define PTED_VA_EXEC_M		0x40

static inline u_int32_t
PTED_HID(struct pte_desc *pted)
{
	return (pted->pted_va & PTED_VA_HID_M); 
}

static inline u_int32_t
PTED_PTEGIDX(struct pte_desc *pted)
{
	return (pted->pted_va & PTED_VA_PTEGIDX_M); 
}

static inline u_int32_t
PTED_MANAGED(struct pte_desc *pted)
{
	return (pted->pted_va & PTED_VA_MANAGED_M); 
}

static inline u_int32_t
PTED_WIRED(struct pte_desc *pted)
{
	return (pted->pted_va & PTED_VA_WIRED_M); 
}

static inline u_int32_t
PTED_VALID(struct pte_desc *pted)
{
	if (ppc_proc_is_64b)
		return (pted->p.pted_pte64.pte_hi & PTE_VALID_64);
	else 
		return (pted->p.pted_pte32.pte_hi & PTE_VALID_32);
}

/*
 * PV entries -
 * manipulate the physical to virtual translations for the entire system.
 * 
 * QUESTION: should all mapped memory be stored in PV tables? Or
 * is it alright to only store "ram" memory. Currently device mappings
 * are not stored.
 * It makes sense to pre-allocate mappings for all of "ram" memory, since
 * it is likely that it will be mapped at some point, but would it also
 * make sense to use a tree/table like is use for pmap to store device
 * mappings?
 * Further notes: It seems that the PV table is only used for pmap_protect
 * and other paging related operations. Given this, it is not necessary
 * to store any pmap_kernel() entries in PV tables and does not make
 * sense to store device mappings in PV either.
 *
 * Note: unlike other powerpc pmap designs, the array is only an array
 * of pointers. Since the same structure is used for holding information
 * in the VP table, the PV table, and for kernel mappings, the wired entries.
 * Allocate one data structure to hold all of the info, instead of replicating
 * it multiple times.
 *
 * One issue of making this a single data structure is that two pointers are
 * wasted for every page which does not map ram (device mappings), this 
 * should be a low percentage of mapped pages in the system, so should not
 * have too noticable unnecessary ram consumption.
 */

void
pmap_enter_pv(struct pte_desc *pted, struct vm_page *pg)
{
	if (__predict_false(!pmap_initialized)) {
		return;
	}

	LIST_INSERT_HEAD(&(pg->mdpage.pv_list), pted, pted_pv_list);
	pted->pted_va |= PTED_VA_MANAGED_M;
}

void
pmap_remove_pv(struct pte_desc *pted)
{
	pted->pted_va &= ~PTED_VA_MANAGED_M;
	LIST_REMOVE(pted, pted_pv_list);
}


/* PTE_CHG_32 == PTE_CHG_64 */
/* PTE_REF_32 == PTE_REF_64 */
static __inline u_int
pmap_pte2flags(u_int32_t pte)
{
	return (((pte & PTE_REF_32) ? PG_PMAP_REF : 0) |
	    ((pte & PTE_CHG_32) ? PG_PMAP_MOD : 0));
}

static __inline u_int
pmap_flags2pte(u_int32_t flags)
{
	return (((flags & PG_PMAP_REF) ? PTE_REF_32 : 0) |
	    ((flags & PG_PMAP_MOD) ? PTE_CHG_32 : 0));
}

void
pmap_attr_save(paddr_t pa, u_int32_t bits)
{
	struct vm_page *pg;

	pg = PHYS_TO_VM_PAGE(pa);
	if (pg == NULL)
		return;

	atomic_setbits_int(&pg->pg_flags,  pmap_pte2flags(bits));
}

int
pmap_enter(pmap_t pm, vaddr_t va, paddr_t pa, vm_prot_t prot, int flags)
{
	struct pte_desc *pted;
	struct vm_page *pg;
	boolean_t nocache = (pa & PMAP_NOCACHE) != 0;
	boolean_t wt = (pa & PMAP_WT) != 0;
	int need_sync = 0;
	int cache, error = 0;

	KASSERT(!(wt && nocache));
	pa &= PMAP_PA_MASK;

	PMAP_VP_LOCK(pm);
	pted = pmap_vp_lookup(pm, va);
	if (pted && PTED_VALID(pted)) {
		pmap_remove_pted(pm, pted);
		/* we lost our pted if it was user */
		if (pm != pmap_kernel())
			pted = pmap_vp_lookup(pm, va);
	}

	pm->pm_stats.resident_count++;

	/* Do not have pted for this, get one and put it in VP */
	if (pted == NULL) {
		pted = pool_get(&pmap_pted_pool, PR_NOWAIT | PR_ZERO);
		if (pted == NULL) {
			if ((flags & PMAP_CANFAIL) == 0) {
				error = ENOMEM;
				goto out;
			}
			panic("pmap_enter: failed to allocate pted");
		}
		error = pmap_vp_enter(pm, va, pted, flags);
		if (error) {
			pool_put(&pmap_pted_pool, pted);
			goto out;
		}
	}

	pg = PHYS_TO_VM_PAGE(pa);
	if (wt)
		cache = PMAP_CACHE_WT;
	else if (pg != NULL && !(pg->pg_flags & PG_DEV) && !nocache)
		cache = PMAP_CACHE_WB;
	else
		cache = PMAP_CACHE_CI;

	/* Calculate PTE */
	if (ppc_proc_is_64b)
		pmap_fill_pte64(pm, va, pa, pted, prot, cache);
	else
		pmap_fill_pte32(pm, va, pa, pted, prot, cache);

	if (pg != NULL) {
		pmap_enter_pv(pted, pg); /* only managed mem */
	}

	/*
	 * Insert into HTAB
	 * We were told to map the page, probably called from vm_fault,
	 * so map the page!
	 */
	if (ppc_proc_is_64b)
		pte_insert64(pted);
	else
		pte_insert32(pted);

        if (prot & PROT_EXEC) {
		u_int sn = VP_SR(va);

        	pm->pm_exec[sn]++;
		if (pm->pm_sr[sn] & SR_NOEXEC)
			pm->pm_sr[sn] &= ~SR_NOEXEC;

		if (pg != NULL) {
			need_sync = ((pg->pg_flags & PG_PMAP_EXE) == 0);
			if (prot & PROT_WRITE)
				atomic_clearbits_int(&pg->pg_flags,
				    PG_PMAP_EXE);
			else
				atomic_setbits_int(&pg->pg_flags,
				    PG_PMAP_EXE);
		} else
			need_sync = 1;
	} else {
		/*
		 * Should we be paranoid about writeable non-exec 
		 * mappings ? if so, clear the exec tag
		 */
		if ((prot & PROT_WRITE) && (pg != NULL))
			atomic_clearbits_int(&pg->pg_flags, PG_PMAP_EXE);
	}

	/* only instruction sync executable pages */
	if (need_sync)
		pmap_syncicache_user_virt(pm, va);

out:
	PMAP_VP_UNLOCK(pm);
	return (error);
}

/*
 * Remove the given range of mapping entries.
 */
void
pmap_remove(pmap_t pm, vaddr_t sva, vaddr_t eva)
{
	struct pte_desc *pted;
	vaddr_t va;

	KERNEL_LOCK();
	PMAP_VP_LOCK(pm);
	for (va = sva; va < eva; va += PAGE_SIZE) {
		pted = pmap_vp_lookup(pm, va);
		if (pted && PTED_VALID(pted))
			pmap_remove_pted(pm, pted);
	}
	PMAP_VP_UNLOCK(pm);
	KERNEL_UNLOCK();
}

/*
 * remove a single mapping, notice that this code is O(1)
 */
void
pmap_remove_pted(pmap_t pm, struct pte_desc *pted)
{
	void *pte;
	int s;

	KASSERT(pm == pted->pted_pmap);
	PMAP_VP_ASSERT_LOCKED(pm);

	pm->pm_stats.resident_count--;

	PMAP_HASH_LOCK(s);
	if ((pte = pmap_ptedinhash(pted)) != NULL)
		pte_zap(pte, pted);
	PMAP_HASH_UNLOCK(s);

	if (pted->pted_va & PTED_VA_EXEC_M) {
		u_int sn = VP_SR(pted->pted_va);

		pted->pted_va &= ~PTED_VA_EXEC_M;
		pm->pm_exec[sn]--;
		if (pm->pm_exec[sn] == 0)
			pm->pm_sr[sn] |= SR_NOEXEC;
	}

	if (ppc_proc_is_64b)
		pted->p.pted_pte64.pte_hi &= ~PTE_VALID_64;
	else
		pted->p.pted_pte32.pte_hi &= ~PTE_VALID_32;

	if (PTED_MANAGED(pted))
		pmap_remove_pv(pted);

	if (pm != pmap_kernel()) {
		(void)pmap_vp_remove(pm, pted->pted_va);
		pool_put(&pmap_pted_pool, pted);
	}
}

/*
 * Enter a kernel mapping for the given page.
 * kernel mappings have a larger set of prerequisites than normal mappings.
 * 
 * 1. no memory should be allocated to create a kernel mapping.
 * 2. a vp mapping should already exist, even if invalid. (see 1)
 * 3. all vp tree mappings should already exist (see 1)
 * 
 */
void
pmap_kenter_pa(vaddr_t va, paddr_t pa, vm_prot_t prot)
{
	struct pte_desc *pted;
	struct vm_page *pg;
	boolean_t nocache = (pa & PMAP_NOCACHE) != 0;
	boolean_t wt = (pa & PMAP_WT) != 0;
	pmap_t pm;
	int cache;

	KASSERT(!(wt && nocache));
	pa &= PMAP_PA_MASK;

	pm = pmap_kernel();

	pted = pmap_vp_lookup(pm, va);
	if (pted && PTED_VALID(pted))
		pmap_remove_pted(pm, pted); /* pted is reused */

	pm->pm_stats.resident_count++;

	if (prot & PROT_WRITE) {
		pg = PHYS_TO_VM_PAGE(pa);
		if (pg != NULL)
			atomic_clearbits_int(&pg->pg_flags, PG_PMAP_EXE);
	}

	/* Do not have pted for this, get one and put it in VP */
	if (pted == NULL) {
		panic("pted not preallocated in pmap_kernel() va %lx pa %lx",
		    va, pa);
	}

	pg = PHYS_TO_VM_PAGE(pa);
	if (wt)
		cache = PMAP_CACHE_WT;
	else if (pg != NULL && !(pg->pg_flags & PG_DEV) && !nocache)
		cache = PMAP_CACHE_WB;
	else
		cache = PMAP_CACHE_CI;

	/* Calculate PTE */
	if (ppc_proc_is_64b)
		pmap_fill_pte64(pm, va, pa, pted, prot, cache);
	else
		pmap_fill_pte32(pm, va, pa, pted, prot, cache);

	/*
	 * Insert into HTAB
	 * We were told to map the page, probably called from vm_fault,
	 * so map the page!
	 */
	if (ppc_proc_is_64b)
		pte_insert64(pted);
	else
		pte_insert32(pted);

	pted->pted_va |= PTED_VA_WIRED_M;

        if (prot & PROT_EXEC) {
		u_int sn = VP_SR(va);

        	pm->pm_exec[sn]++;
		if (pm->pm_sr[sn] & SR_NOEXEC)
			pm->pm_sr[sn] &= ~SR_NOEXEC;
	}
}

/*
 * remove kernel (pmap_kernel()) mappings
 */
void
pmap_kremove(vaddr_t va, vsize_t len)
{
	struct pte_desc *pted;

	for (len >>= PAGE_SHIFT; len > 0; len--, va += PAGE_SIZE) {
		pted = pmap_vp_lookup(pmap_kernel(), va);
		if (pted && PTED_VALID(pted))
			pmap_remove_pted(pmap_kernel(), pted);
	}
}

static inline void *
pmap_ptedinhash(struct pte_desc *pted)
{
	vaddr_t va = pted->pted_va & ~PAGE_MASK;
	pmap_t pm = pted->pted_pmap;
	int sr, idx;

	sr = ptesr(pm->pm_sr, va);
	idx = pteidx(sr, va);

	if (ppc_proc_is_64b) {
		struct pte_64 *pte = pmap_ptable64;

		pte += (idx ^ (PTED_HID(pted) ? pmap_ptab_mask : 0)) * 8;
		pte += PTED_PTEGIDX(pted);

		/*
		 * We now have the pointer to where it will be, if it is
		 * currently mapped. If the mapping was thrown away in
		 * exchange for another page mapping, then this page is
		 * not currently in the HASH.
		 */
		if ((pted->p.pted_pte64.pte_hi |
		    (PTED_HID(pted) ? PTE_HID_64 : 0)) == pte->pte_hi)
			return (pte);
	} else {
		struct pte_32 *pte = pmap_ptable32;

		pte += (idx ^ (PTED_HID(pted) ? pmap_ptab_mask : 0)) * 8;
		pte += PTED_PTEGIDX(pted);

		/*
		 * We now have the pointer to where it will be, if it is
		 * currently mapped. If the mapping was thrown away in
		 * exchange for another page mapping, then this page is
		 * not currently in the HASH.
		 */
		if ((pted->p.pted_pte32.pte_hi |
		    (PTED_HID(pted) ? PTE_HID_32 : 0)) == pte->pte_hi)
			return (pte);
	}

	return (NULL);
}

/*
 * Delete a Page Table Entry, section 7.6.3.3.
 *
 * Note: pte must be locked.
 */
void
pte_del(void *pte, vaddr_t va)
{
	if (ppc_proc_is_64b)
		((struct pte_64 *)pte)->pte_hi &= ~PTE_VALID_64;
	else
		((struct pte_32 *)pte)->pte_hi &= ~PTE_VALID_32;

	sync();		/* Ensure update completed. */
	tlbie(va);	/* Invalidate old translation. */
	eieio();	/* Order tlbie before tlbsync. */
	tlbsync();	/* Ensure tlbie completed on all processors. */
	sync();		/* Ensure tlbsync and update completed. */
}

void
pte_zap(void *pte, struct pte_desc *pted)
{
	pte_del(pte, pted->pted_va);

	if (!PTED_MANAGED(pted))
		return;

	if (ppc_proc_is_64b) {
		pmap_attr_save(pted->p.pted_pte64.pte_lo & PTE_RPGN_64,
		    ((struct pte_64 *)pte)->pte_lo & (PTE_REF_64|PTE_CHG_64));
	} else {
		pmap_attr_save(pted->p.pted_pte32.pte_lo & PTE_RPGN_32,
		    ((struct pte_32 *)pte)->pte_lo & (PTE_REF_32|PTE_CHG_32));
	}
}

/*
 * What about execution control? Even at only a segment granularity.
 */
void
pmap_fill_pte64(pmap_t pm, vaddr_t va, paddr_t pa, struct pte_desc *pted,
	vm_prot_t prot, int cache)
{
	sr_t sr;
	struct pte_64 *pte64;

	sr = ptesr(pm->pm_sr, va);
	pte64 = &pted->p.pted_pte64;

	pte64->pte_hi = (((u_int64_t)sr & SR_VSID) <<
	   PTE_VSID_SHIFT_64) |
	    ((va >> ADDR_API_SHIFT_64) & PTE_API_64) | PTE_VALID_64;
	pte64->pte_lo = (pa & PTE_RPGN_64);


	if (cache == PMAP_CACHE_WB)
		pte64->pte_lo |= PTE_M_64;
	else if (cache == PMAP_CACHE_WT)
		pte64->pte_lo |= (PTE_W_64 | PTE_M_64);
	else
		pte64->pte_lo |= (PTE_M_64 | PTE_I_64 | PTE_G_64);

	if (prot & PROT_WRITE)
		pte64->pte_lo |= PTE_RW_64;
	else
		pte64->pte_lo |= PTE_RO_64;

	pted->pted_va = va & ~PAGE_MASK;

	if (prot & PROT_EXEC)
		pted->pted_va  |= PTED_VA_EXEC_M;
	else
		pte64->pte_lo |= PTE_N_64;

	pted->pted_pmap = pm;
}

/*
 * What about execution control? Even at only a segment granularity.
 */
void
pmap_fill_pte32(pmap_t pm, vaddr_t va, paddr_t pa, struct pte_desc *pted,
	vm_prot_t prot, int cache)
{
	sr_t sr;
	struct pte_32 *pte32;

	sr = ptesr(pm->pm_sr, va);
	pte32 = &pted->p.pted_pte32;

	pte32->pte_hi = ((sr & SR_VSID) << PTE_VSID_SHIFT_32) |
	    ((va >> ADDR_API_SHIFT_32) & PTE_API_32) | PTE_VALID_32;
	pte32->pte_lo = (pa & PTE_RPGN_32);

	if (cache == PMAP_CACHE_WB)
		pte32->pte_lo |= PTE_M_32;
	else if (cache == PMAP_CACHE_WT)
		pte32->pte_lo |= (PTE_W_32 | PTE_M_32);
	else
		pte32->pte_lo |= (PTE_M_32 | PTE_I_32 | PTE_G_32);

	if (prot & PROT_WRITE)
		pte32->pte_lo |= PTE_RW_32;
	else
		pte32->pte_lo |= PTE_RO_32;

	pted->pted_va = va & ~PAGE_MASK;

	/* XXX Per-page execution control. */
	if (prot & PROT_EXEC)
		pted->pted_va  |= PTED_VA_EXEC_M;

	pted->pted_pmap = pm;
}

int
pmap_test_attrs(struct vm_page *pg, u_int flagbit)
{
	u_int bits;
	struct pte_desc *pted;
	u_int ptebit = pmap_flags2pte(flagbit);
	int s;

	/* PTE_CHG_32 == PTE_CHG_64 */
	/* PTE_REF_32 == PTE_REF_64 */

	bits = pg->pg_flags & flagbit;
	if ((bits == flagbit))
		return bits;

	LIST_FOREACH(pted, &(pg->mdpage.pv_list), pted_pv_list) {
		void *pte;

		PMAP_HASH_LOCK(s);
		if ((pte = pmap_ptedinhash(pted)) != NULL) {
			if (ppc_proc_is_64b) {
				struct pte_64 *ptp64 = pte;
				bits |=	pmap_pte2flags(ptp64->pte_lo & ptebit);
			} else {
				struct pte_32 *ptp32 = pte;
				bits |=	pmap_pte2flags(ptp32->pte_lo & ptebit);
			}
		}
		PMAP_HASH_UNLOCK(s);

		if (bits == flagbit)
			break;
	}

	atomic_setbits_int(&pg->pg_flags,  bits);

	return bits;
}
int
pmap_clear_attrs(struct vm_page *pg, u_int flagbit)
{
	u_int bits;
	struct pte_desc *pted;
	u_int ptebit = pmap_flags2pte(flagbit);
	int s;

	/* PTE_CHG_32 == PTE_CHG_64 */
	/* PTE_REF_32 == PTE_REF_64 */

	bits = pg->pg_flags & flagbit;

	LIST_FOREACH(pted, &(pg->mdpage.pv_list), pted_pv_list) {
		void *pte;

		PMAP_HASH_LOCK(s);
		if ((pte = pmap_ptedinhash(pted)) != NULL) {
			if (ppc_proc_is_64b) {
				struct pte_64 *ptp64 = pte;

				bits |=	pmap_pte2flags(ptp64->pte_lo & ptebit);

				pte_del(ptp64, pted->pted_va);

				ptp64->pte_lo &= ~ptebit;
				eieio();
				ptp64->pte_hi |= PTE_VALID_64;
				sync();
			} else {
				struct pte_32 *ptp32 = pte;

				bits |=	pmap_pte2flags(ptp32->pte_lo & ptebit);

				pte_del(ptp32, pted->pted_va);

				ptp32->pte_lo &= ~ptebit;
				eieio();
				ptp32->pte_hi |= PTE_VALID_32;
				sync();
			}
		}
		PMAP_HASH_UNLOCK(s);
	}

	/*
	 * this is done a second time, because while walking the list
	 * a bit could have been promoted via pmap_attr_save()
	 */
	bits |= pg->pg_flags & flagbit;
	atomic_clearbits_int(&pg->pg_flags,  flagbit);

	return bits;
}

/*
 * Garbage collects the physical map system for pages which are 
 * no longer used. Success need not be guaranteed -- that is, there
 * may well be pages which are not referenced, but others may be collected
 * Called by the pageout daemon when pages are scarce.
 */
void
pmap_collect(pmap_t pm)
{
	/* This could return unused v->p table layers which 
	 * are empty.
	 * could malicious programs allocate memory and eat
	 * these wired pages? These are allocated via pool.
	 * Are there pool functions which could be called
	 * to lower the pool usage here?
	 */
}

/*
 * Fill the given physical page with zeros.
 */
void
pmap_zero_page(struct vm_page *pg)
{
	vaddr_t va = pmap_map_direct(pg);
	int i;

	/*
	 * Loop over & zero cache lines.  This code assumes that 64-bit
	 * CPUs have 128-byte cache lines.  We explicitely use ``dcbzl''
	 * here because we do not clear the DCBZ_SIZE bit of the HID5
	 * register in order to be compatible with code using ``dcbz''
	 * and assuming that cache line size is 32.
	 */
	if (ppc_proc_is_64b) {
		for (i = 0; i < PAGE_SIZE; i += 128)
			asm volatile ("dcbzl 0,%0" :: "r"(va + i));
		return;
	}

	for (i = 0; i < PAGE_SIZE; i += CACHELINESIZE)
		asm volatile ("dcbz 0,%0" :: "r"(va + i));
}

/*
 * Copy a page.
 */
void
pmap_copy_page(struct vm_page *srcpg, struct vm_page *dstpg)
{
	vaddr_t srcva = pmap_map_direct(srcpg);
	vaddr_t dstva = pmap_map_direct(dstpg);

	memcpy((void *)dstva, (void *)srcva, PAGE_SIZE);
}

int pmap_id_avail = 0;

pmap_t
pmap_create(void)
{
	int i, k, try, tblidx, tbloff;
	int s, seg;
	pmap_t pm;

	pm = pool_get(&pmap_pmap_pool, PR_WAITOK|PR_ZERO);

	pmap_reference(pm);
	PMAP_VP_LOCK_INIT(pm);

	/*
	 * Allocate segment registers for this pmap.
	 * Try not to reuse pmap ids, to spread the hash table usage.
	 */
again:
	for (i = 0; i < NPMAPS; i++) {
		try = pmap_id_avail + i;
		try = try % NPMAPS; /* truncate back into bounds */
		tblidx = try / (8 * sizeof usedsr[0]);
		tbloff = try % (8 * sizeof usedsr[0]);
		if ((usedsr[tblidx] & (1 << tbloff)) == 0) {
			/* pmap create lock? */
			s = splvm();
			if ((usedsr[tblidx] & (1 << tbloff)) == 1) {
				/* entry was stolen out from under us, retry */
				splx(s); /* pmap create unlock */
				goto again;
			}
			usedsr[tblidx] |= (1 << tbloff); 
			pmap_id_avail = try + 1;
			splx(s); /* pmap create unlock */

			seg = try << 4;
			for (k = 0; k < 16; k++)
				pm->pm_sr[k] = (seg + k) | SR_NOEXEC;
			return (pm);
		}
	}
	panic("out of pmap slots");
}

/*
 * Add a reference to a given pmap.
 */
void
pmap_reference(pmap_t pm)
{
	pm->pm_refs++;
}

/*
 * Retire the given pmap from service.
 * Should only be called if the map contains no valid mappings.
 */
void
pmap_destroy(pmap_t pm)
{
	int refs;

	refs = --pm->pm_refs;
	if (refs == -1)
		panic("re-entering pmap_destroy");
	if (refs > 0)
		return;

	/*
	 * reference count is zero, free pmap resources and free pmap.
	 */
	pmap_release(pm);
	pool_put(&pmap_pmap_pool, pm);
}

/*
 * Release any resources held by the given physical map.
 * Called when a pmap initialized by pmap_pinit is being released.
 */
void
pmap_release(pmap_t pm)
{
	int i, tblidx, tbloff;
	int s;

	pmap_vp_destroy(pm);
	i = (pm->pm_sr[0] & SR_VSID) >> 4;
	tblidx = i / (8  * sizeof usedsr[0]);
	tbloff = i % (8  * sizeof usedsr[0]);

	/* LOCK? */
	s = splvm();
	usedsr[tblidx] &= ~(1 << tbloff);
	splx(s);
}

void
pmap_vp_destroy(pmap_t pm)
{
	int i, j;
	struct pmapvp *vp1;
	struct pmapvp *vp2;

	for (i = 0; i < VP_SR_SIZE; i++) {
		vp1 = pm->pm_vp[i];
		if (vp1 == NULL)
			continue;

		for (j = 0; j < VP_IDX1_SIZE; j++) {
			vp2 = vp1->vp[j];
			if (vp2 == NULL)
				continue;
			
			pool_put(&pmap_vp_pool, vp2);
		}
		pm->pm_vp[i] = NULL;
		pool_put(&pmap_vp_pool, vp1);
	}
}

void
pmap_avail_setup(void)
{
	struct mem_region *mp;
	int pmap_physmem;

	ppc_mem_regions(&pmap_mem, &pmap_avail);
	pmap_cnt_avail = 0;
	pmap_physmem = 0;

	ndumpmem = 0;
	for (mp = pmap_mem; mp->size !=0; mp++, ndumpmem++) {
		pmap_physmem += atop(mp->size);
		dumpmem[ndumpmem].start = atop(mp->start);
		dumpmem[ndumpmem].end = atop(mp->start + mp->size);
	}

	if (physmem == 0)
		physmem = pmap_physmem;

	for (mp = pmap_avail; mp->size !=0 ; mp++) {
		if (physmaxaddr <  mp->start + mp->size)
			physmaxaddr = mp->start + mp->size;
	}

	for (mp = pmap_avail; mp->size !=0; mp++)
		pmap_cnt_avail += 1;
}

void
pmap_avail_fixup(void)
{
	struct mem_region *mp;
	u_int32_t align;
	u_int32_t end;

	mp = pmap_avail;
	while(mp->size !=0) {
		align = round_page(mp->start);
		if (mp->start != align) {
			pmap_remove_avail(mp->start, align);
			mp = pmap_avail;
			continue;
		}
		end = mp->start+mp->size;
		align = trunc_page(end);
		if (end != align) {
			pmap_remove_avail(align, end);
			mp = pmap_avail;
			continue;
		}
		mp++;
	}
}

/* remove a given region from avail memory */
void
pmap_remove_avail(paddr_t base, paddr_t end)
{
	struct mem_region *mp;
	int i;
	int mpend;

	/* remove given region from available */
	for (mp = pmap_avail; mp->size; mp++) {
		/*
		 * Check if this region holds all of the region
		 */
		mpend = mp->start + mp->size;
		if (base > mpend) {
			continue;
		}
		if (base <= mp->start) {
			if (end <= mp->start)
				break; /* region not present -??? */

			if (end >= mpend) {
				/* covers whole region */
				/* shorten */
				for (i = mp - pmap_avail;
				    i < pmap_cnt_avail;
				    i++) {
					pmap_avail[i] = pmap_avail[i+1];
				}
				pmap_cnt_avail--;
				pmap_avail[pmap_cnt_avail].size = 0;
			} else {
				mp->start = end;
				mp->size = mpend - end;
			}
		} else {
			/* start after the beginning */
			if (end >= mpend) {
				/* just truncate */
				mp->size = base - mp->start;
			} else {
				/* split */
				for (i = pmap_cnt_avail;
				    i > (mp - pmap_avail);
				    i--) {
					pmap_avail[i] = pmap_avail[i - 1];
				}
				pmap_cnt_avail++;
				mp->size = base - mp->start;
				mp++;
				mp->start = end;
				mp->size = mpend - end;
			}
		}
	}
	for (mp = pmap_allocated; mp->size != 0; mp++) {
		if (base < mp->start) {
			if (end == mp->start) {
				mp->start = base;
				mp->size += end - base;
				break;
			}
			/* lengthen */
			for (i = pmap_cnt_allocated; i > (mp - pmap_allocated);
			    i--) {
				pmap_allocated[i] = pmap_allocated[i - 1];
			}
			pmap_cnt_allocated++;
			mp->start = base;
			mp->size = end - base;
			return;
		}
		if (base == (mp->start + mp->size)) {
			mp->size += end - base;
			return;
		}
	}
	if (mp->size == 0) {
		mp->start = base;
		mp->size  = end - base;
		pmap_cnt_allocated++;
	}
}

void *
pmap_steal_avail(size_t size, int align)
{
	struct mem_region *mp;
	int start;
	int remsize;

	for (mp = pmap_avail; mp->size; mp++) {
		if (mp->size > size) {
			start = (mp->start + (align -1)) & ~(align -1);
			remsize = mp->size - (start - mp->start); 
			if (remsize >= 0) {
				pmap_remove_avail(start, start+size);
				return (void *)start;
			}
		}
	}
	panic ("unable to allocate region with size %zx align %x",
	    size, align);
}

/*
 * Similar to pmap_steal_avail, but operating on vm_physmem since
 * uvm_page_physload() has been called.
 */
vaddr_t
pmap_steal_memory(vsize_t size, vaddr_t *start, vaddr_t *end)
{
	int segno;
	u_int npg;
	vaddr_t va;
	paddr_t pa;
	struct vm_physseg *seg;

	size = round_page(size);
	npg = atop(size);

	for (segno = 0, seg = vm_physmem; segno < vm_nphysseg; segno++, seg++) {
		if (seg->avail_end - seg->avail_start < npg)
			continue;
		/*
		 * We can only steal at an ``unused'' segment boundary,
		 * i.e. either at the start or at the end.
		 */
		if (seg->avail_start == seg->start ||
		    seg->avail_end == seg->end)
			break;
	}
	if (segno == vm_nphysseg)
		va = 0;
	else {
		if (seg->avail_start == seg->start) {
			pa = ptoa(seg->avail_start);
			seg->avail_start += npg;
			seg->start += npg;
		} else {
			pa = ptoa(seg->avail_end) - size;
			seg->avail_end -= npg;
			seg->end -= npg;
		}
		/*
		 * If all the segment has been consumed now, remove it.
		 * Note that the crash dump code still knows about it
		 * and will dump it correctly.
		 */
		if (seg->start == seg->end) {
			if (vm_nphysseg-- == 1)
				panic("pmap_steal_memory: out of memory");
			while (segno < vm_nphysseg) {
				seg[0] = seg[1]; /* struct copy */
				seg++;
				segno++;
			}
		}

		va = (vaddr_t)pa;	/* 1:1 mapping */
		bzero((void *)va, size);
	}

	if (start != NULL)
		*start = VM_MIN_KERNEL_ADDRESS;
	if (end != NULL)
		*end = VM_MAX_KERNEL_ADDRESS;

	return (va);
}

void *msgbuf_addr;

/*
 * Initialize pmap setup.
 * ALL of the code which deals with avail needs rewritten as an actual
 * memory allocation.
 */ 
void
pmap_bootstrap(u_int kernelstart, u_int kernelend)
{
	struct mem_region *mp;
	int i, k;
	struct pmapvp *vp1;
	struct pmapvp *vp2;

	/*
	 * set the page size (default value is 4K which is ok)
	 */
	uvm_setpagesize();

	/*
	 * Get memory.
	 */
	pmap_avail_setup();

	/*
	 * Page align all regions.
	 * Non-page memory isn't very interesting to us.
	 * Also, sort the entries for ascending addresses.
	 */
	kernelstart = trunc_page(kernelstart);
	kernelend = round_page(kernelend);
	pmap_remove_avail(kernelstart, kernelend);

	msgbuf_addr = pmap_steal_avail(MSGBUFSIZE,4);

#ifdef DEBUG
	for (mp = pmap_avail; mp->size; mp++) {
		bzero((void *)mp->start, mp->size);
	}
#endif

#define HTABENTS_32 1024
#define HTABENTS_64 2048

	if (ppc_proc_is_64b) { 
		pmap_ptab_cnt = HTABENTS_64;
		while (pmap_ptab_cnt * 2 < physmem)
			pmap_ptab_cnt <<= 1;
	} else {
		pmap_ptab_cnt = HTABENTS_32;
		while (HTABSIZE_32 < (ptoa(physmem) >> 7))
			pmap_ptab_cnt <<= 1;
	}
	/*
	 * allocate suitably aligned memory for HTAB
	 */
	if (ppc_proc_is_64b) {
		pmap_ptable64 = pmap_steal_avail(HTABMEMSZ_64, HTABMEMSZ_64);
		bzero((void *)pmap_ptable64, HTABMEMSZ_64);
		pmap_ptab_mask = pmap_ptab_cnt - 1;
	} else {
		pmap_ptable32 = pmap_steal_avail(HTABSIZE_32, HTABSIZE_32);
		bzero((void *)pmap_ptable32, HTABSIZE_32);
		pmap_ptab_mask = pmap_ptab_cnt - 1;
	}

	/* allocate v->p mappings for pmap_kernel() */
	for (i = 0; i < VP_SR_SIZE; i++) {
		pmap_kernel()->pm_vp[i] = NULL;
	}
	vp1 = pmap_steal_avail(sizeof (struct pmapvp), 4);
	bzero (vp1, sizeof(struct pmapvp));
	pmap_kernel()->pm_vp[PPC_KERNEL_SR] = vp1;
	for (i = 0; i < VP_IDX1_SIZE; i++) {
		vp2 = vp1->vp[i] = pmap_steal_avail(sizeof (struct pmapvp), 4);
		bzero (vp2, sizeof(struct pmapvp));
		for (k = 0; k < VP_IDX2_SIZE; k++) {
			struct pte_desc *pted;
			pted = pmap_steal_avail(sizeof (struct pte_desc), 4);
			bzero (pted, sizeof (struct pte_desc));
			vp2->vp[k] = pted;
		}
	}

	/*
	 * Initialize kernel pmap and hardware.
	 */
#if NPMAPS >= PPC_KERNEL_SEGMENT / 16
	usedsr[PPC_KERNEL_SEGMENT / 16 / (sizeof usedsr[0] * 8)]
		|= 1 << ((PPC_KERNEL_SEGMENT / 16) % (sizeof usedsr[0] * 8));
#endif
	for (i = 0; i < 16; i++)
		pmap_kernel()->pm_sr[i] = (PPC_KERNEL_SEG0 + i) | SR_NOEXEC;

	if (ppc_nobat) {
		vp1 = pmap_steal_avail(sizeof (struct pmapvp), 4);
		bzero (vp1, sizeof(struct pmapvp));
		pmap_kernel()->pm_vp[0] = vp1;
		for (i = 0; i < VP_IDX1_SIZE; i++) {
			vp2 = vp1->vp[i] =
			    pmap_steal_avail(sizeof (struct pmapvp), 4);
			bzero (vp2, sizeof(struct pmapvp));
			for (k = 0; k < VP_IDX2_SIZE; k++) {
				struct pte_desc *pted;
				pted = pmap_steal_avail(sizeof (struct pte_desc), 4);
				bzero (pted, sizeof (struct pte_desc));
				vp2->vp[k] = pted;
			}
		}

		/* first segment contains executable pages */
		pmap_kernel()->pm_exec[0]++;
		pmap_kernel()->pm_sr[0] &= ~SR_NOEXEC;
	} else {
		/*
		 * Setup fixed BAT registers.
		 *
		 * Note that we still run in real mode, and the BAT
		 * registers were cleared in cpu_bootstrap().
		 */
		battable[0].batl = BATL(0x00000000, BAT_M);
		if (physmem > atop(0x08000000))
			battable[0].batu = BATU(0x00000000, BAT_BL_256M);
		else
			battable[0].batu = BATU(0x00000000, BAT_BL_128M);

		/* Map physical memory with BATs. */
		if (physmem > atop(0x10000000)) {
			battable[0x1].batl = BATL(0x10000000, BAT_M);
			battable[0x1].batu = BATU(0x10000000, BAT_BL_256M);
		}
		if (physmem > atop(0x20000000)) {
			battable[0x2].batl = BATL(0x20000000, BAT_M);
			battable[0x2].batu = BATU(0x20000000, BAT_BL_256M);
		}
		if (physmem > atop(0x30000000)) {
			battable[0x3].batl = BATL(0x30000000, BAT_M);
			battable[0x3].batu = BATU(0x30000000, BAT_BL_256M);
		}
		if (physmem > atop(0x40000000)) {
			battable[0x4].batl = BATL(0x40000000, BAT_M);
			battable[0x4].batu = BATU(0x40000000, BAT_BL_256M);
		}
		if (physmem > atop(0x50000000)) {
			battable[0x5].batl = BATL(0x50000000, BAT_M);
			battable[0x5].batu = BATU(0x50000000, BAT_BL_256M);
		}
		if (physmem > atop(0x60000000)) {
			battable[0x6].batl = BATL(0x60000000, BAT_M);
			battable[0x6].batu = BATU(0x60000000, BAT_BL_256M);
		}
		if (physmem > atop(0x70000000)) {
			battable[0x7].batl = BATL(0x70000000, BAT_M);
			battable[0x7].batu = BATU(0x70000000, BAT_BL_256M);
		}
	}

	ppc_kvm_stolen += reserve_dumppages( (caddr_t)(VM_MIN_KERNEL_ADDRESS +
	    ppc_kvm_stolen));

	pmap_avail_fixup();
	for (mp = pmap_avail; mp->size; mp++) {
		if (mp->start > 0x80000000)
			continue;
		if (mp->start + mp->size > 0x80000000)
			mp->size = 0x80000000 - mp->start;
		uvm_page_physload(atop(mp->start), atop(mp->start+mp->size),
		    atop(mp->start), atop(mp->start+mp->size), 0);
	}
}

void
pmap_enable_mmu(void)
{
	uint32_t scratch, sdr1;
	int i;

	if (!ppc_nobat) {
		/* DBAT0 used for initial segment */
		ppc_mtdbat0l(battable[0].batl);
		ppc_mtdbat0u(battable[0].batu);

		/* IBAT0 only covering the kernel .text */
		ppc_mtibat0l(battable[0].batl);
		ppc_mtibat0u(BATU(0x00000000, BAT_BL_8M));
	}

	for (i = 0; i < 16; i++)
		ppc_mtsrin(PPC_KERNEL_SEG0 + i, i << ADDR_SR_SHIFT);

	if (ppc_proc_is_64b)
		sdr1 = (uint32_t)pmap_ptable64 | HTABSIZE_64;
	else
		sdr1 = (uint32_t)pmap_ptable32 | (pmap_ptab_mask >> 10);

	asm volatile ("sync; mtsdr1 %0; isync" :: "r"(sdr1));
	tlbia();

	asm volatile ("eieio; mfmsr %0; ori %0,%0,%1; mtmsr %0; sync; isync"
	    : "=r"(scratch) : "K"(PSL_IR|PSL_DR|PSL_ME|PSL_RI));
}

/*
 * activate a pmap entry
 * NOOP on powerpc, all PTE entries exist in the same hash table.
 * Segment registers are filled on exit to user mode.
 */
void
pmap_activate(struct proc *p)
{
}

/*
 * deactivate a pmap entry
 * NOOP on powerpc
 */
void
pmap_deactivate(struct proc *p)
{
}

/*
 * pmap_extract: extract a PA for the given VA
 */

boolean_t
pmap_extract(pmap_t pm, vaddr_t va, paddr_t *pa)
{
	struct pte_desc *pted;

	if (pm == pmap_kernel() && va < physmaxaddr) {
		*pa = va;
		return TRUE;
	}

	PMAP_VP_LOCK(pm);
	pted = pmap_vp_lookup(pm, va);
	if (pted == NULL || !PTED_VALID(pted)) {
		PMAP_VP_UNLOCK(pm);
		return FALSE;
	}

	if (ppc_proc_is_64b)
		*pa = (pted->p.pted_pte64.pte_lo & PTE_RPGN_64) |
		    (va & ~PTE_RPGN_64);
	else
		*pa = (pted->p.pted_pte32.pte_lo & PTE_RPGN_32) |
		    (va & ~PTE_RPGN_32);

	PMAP_VP_UNLOCK(pm);
	return TRUE;
}

u_int32_t
pmap_setusr(pmap_t pm, vaddr_t va)
{
	u_int32_t sr;
	u_int32_t oldsr;

	sr = ptesr(pm->pm_sr, va);

	/* user address range lock?? */
	asm volatile ("mfsr %0,%1" : "=r" (oldsr): "n"(PPC_USER_SR));
	asm volatile ("isync; mtsr %0,%1; isync" :: "n"(PPC_USER_SR), "r"(sr));
	return oldsr;
}

void
pmap_popusr(u_int32_t sr)
{
	asm volatile ("isync; mtsr %0,%1; isync"
	    :: "n"(PPC_USER_SR), "r"(sr));
}

int
copyin(const void *udaddr, void *kaddr, size_t len)
{
	void *p;
	size_t l;
	u_int32_t oldsr;
	faultbuf env;
	void *oldh = curpcb->pcb_onfault;

	while (len > 0) {
		p = PPC_USER_ADDR + ((u_int)udaddr & ~PPC_SEGMENT_MASK);
		l = (PPC_USER_ADDR + PPC_SEGMENT_LENGTH) - p;
		if (l > len)
			l = len;
		oldsr = pmap_setusr(curpcb->pcb_pm, (vaddr_t)udaddr);
		if (setfault(&env)) {
			pmap_popusr(oldsr);
			curpcb->pcb_onfault = oldh;
			return EFAULT;
		}
		bcopy(p, kaddr, l);
		pmap_popusr(oldsr);
		udaddr += l;
		kaddr += l;
		len -= l;
	}
	curpcb->pcb_onfault = oldh;
	return 0;
}

int
copyout(const void *kaddr, void *udaddr, size_t len)
{
	void *p;
	size_t l;
	u_int32_t oldsr;
	faultbuf env;
	void *oldh = curpcb->pcb_onfault;

	while (len > 0) {
		p = PPC_USER_ADDR + ((u_int)udaddr & ~PPC_SEGMENT_MASK);
		l = (PPC_USER_ADDR + PPC_SEGMENT_LENGTH) - p;
		if (l > len)
			l = len;
		oldsr = pmap_setusr(curpcb->pcb_pm, (vaddr_t)udaddr);
		if (setfault(&env)) {
			pmap_popusr(oldsr);
			curpcb->pcb_onfault = oldh;
			return EFAULT;
		}

		bcopy(kaddr, p, l);
		pmap_popusr(oldsr);
		udaddr += l;
		kaddr += l;
		len -= l;
	}
	curpcb->pcb_onfault = oldh;
	return 0;
}

int
copyinstr(const void *udaddr, void *kaddr, size_t len, size_t *done)
{
	const u_char *uaddr = udaddr;
	u_char *kp    = kaddr;
	u_char *up;
	u_char c;
	void   *p;
	size_t	 l;
	u_int32_t oldsr;
	int cnt = 0;
	faultbuf env;
	void *oldh = curpcb->pcb_onfault;

	while (len > 0) {
		p = PPC_USER_ADDR + ((u_int)uaddr & ~PPC_SEGMENT_MASK);
		l = (PPC_USER_ADDR + PPC_SEGMENT_LENGTH) - p;
		up = p;
		if (l > len)
			l = len;
		len -= l;
		oldsr = pmap_setusr(curpcb->pcb_pm, (vaddr_t)uaddr);
		if (setfault(&env)) {
			if (done != NULL)
				*done =  cnt;

			curpcb->pcb_onfault = oldh;
			pmap_popusr(oldsr);
			return EFAULT;
		}
		while (l > 0) {
			c = *up;
			*kp = c;
			if (c == 0) {
				if (done != NULL)
					*done = cnt + 1;

				curpcb->pcb_onfault = oldh;
				pmap_popusr(oldsr);
				return 0;
			} 
			up++;
			kp++;
			l--;
			cnt++;
			uaddr++;
		}
		pmap_popusr(oldsr);
	}
	curpcb->pcb_onfault = oldh;
	if (done != NULL)
		*done = cnt;

	return ENAMETOOLONG;
}

int
copyoutstr(const void *kaddr, void *udaddr, size_t len, size_t *done)
{
	u_char *uaddr = (void *)udaddr;
	const u_char *kp    = kaddr;
	u_char *up;
	u_char c;
	void   *p;
	size_t	 l;
	u_int32_t oldsr;
	int cnt = 0;
	faultbuf env;
	void *oldh = curpcb->pcb_onfault;

	while (len > 0) {
		p = PPC_USER_ADDR + ((u_int)uaddr & ~PPC_SEGMENT_MASK);
		l = (PPC_USER_ADDR + PPC_SEGMENT_LENGTH) - p;
		up = p;
		if (l > len)
			l = len;
		len -= l;
		oldsr = pmap_setusr(curpcb->pcb_pm, (vaddr_t)uaddr);
		if (setfault(&env)) {
			if (done != NULL)
				*done =  cnt;

			curpcb->pcb_onfault = oldh;
			pmap_popusr(oldsr);
			return EFAULT;
		}
		while (l > 0) {
			c = *kp;
			*up = c;
			if (c == 0) {
				if (done != NULL)
					*done = cnt + 1;

				curpcb->pcb_onfault = oldh;
				pmap_popusr(oldsr);
				return 0;
			} 
			up++;
			kp++;
			l--;
			cnt++;
			uaddr++;
		}
		pmap_popusr(oldsr);
	}
	curpcb->pcb_onfault = oldh;
	if (done != NULL)
		*done = cnt;

	return ENAMETOOLONG;
}

/*
 * sync instruction cache for user virtual address.
 * The address WAS JUST MAPPED, so we have a VALID USERSPACE mapping
 */
void
pmap_syncicache_user_virt(pmap_t pm, vaddr_t va)
{
	vaddr_t start;
	int oldsr;

	if (pm != pmap_kernel()) {
		start = ((u_int)PPC_USER_ADDR + ((u_int)va &
		    ~PPC_SEGMENT_MASK));
		/* will only ever be page size, will not cross segments */

		/* USER SEGMENT LOCK - MPXXX */
		oldsr = pmap_setusr(pm, va);
	} else {
		start = va; /* flush mapped page */
	}

	syncicache((void *)start, PAGE_SIZE);

	if (pm != pmap_kernel()) {
		pmap_popusr(oldsr);
		/* USER SEGMENT UNLOCK -MPXXX */
	}
}

void
pmap_pted_ro(struct pte_desc *pted, vm_prot_t prot)
{
	if (ppc_proc_is_64b)
		pmap_pted_ro64(pted, prot);
	else
		pmap_pted_ro32(pted, prot);
}

void
pmap_pted_ro64(struct pte_desc *pted, vm_prot_t prot)
{
	pmap_t pm = pted->pted_pmap;
	vaddr_t va = pted->pted_va & ~PAGE_MASK;
	struct vm_page *pg;
	void *pte;
	int s;

	pg = PHYS_TO_VM_PAGE(pted->p.pted_pte64.pte_lo & PTE_RPGN_64);
	if (pg->pg_flags & PG_PMAP_EXE) {
		if ((prot & (PROT_WRITE | PROT_EXEC)) == PROT_WRITE) {
			atomic_clearbits_int(&pg->pg_flags, PG_PMAP_EXE);
		} else {
			pmap_syncicache_user_virt(pm, va);
		}
	}

	pted->p.pted_pte64.pte_lo &= ~PTE_PP_64;
	pted->p.pted_pte64.pte_lo |= PTE_RO_64;

	if ((prot & PROT_EXEC) == 0)
		pted->p.pted_pte64.pte_lo |= PTE_N_64;

	PMAP_HASH_LOCK(s);
	if ((pte = pmap_ptedinhash(pted)) != NULL) {
		struct pte_64 *ptp64 = pte;

		pte_del(ptp64, va);

		if (PTED_MANAGED(pted)) { /* XXX */
			pmap_attr_save(ptp64->pte_lo & PTE_RPGN_64,
			    ptp64->pte_lo & (PTE_REF_64|PTE_CHG_64));
		}

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp64->pte_lo &= ~(PTE_CHG_64|PTE_PP_64);
		ptp64->pte_lo |= PTE_RO_64;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp64->pte_hi |= PTE_VALID_64;
		sync();		/* Ensure updates completed. */
	}
	PMAP_HASH_UNLOCK(s);
}

void
pmap_pted_ro32(struct pte_desc *pted, vm_prot_t prot)
{
	pmap_t pm = pted->pted_pmap;
	vaddr_t va = pted->pted_va & ~PAGE_MASK;
	struct vm_page *pg;
	void *pte;
	int s;

	pg = PHYS_TO_VM_PAGE(pted->p.pted_pte32.pte_lo & PTE_RPGN_32);
	if (pg->pg_flags & PG_PMAP_EXE) {
		if ((prot & (PROT_WRITE | PROT_EXEC)) == PROT_WRITE) {
			atomic_clearbits_int(&pg->pg_flags, PG_PMAP_EXE);
		} else {
			pmap_syncicache_user_virt(pm, va);
		}
	}

	pted->p.pted_pte32.pte_lo &= ~PTE_PP_32;
	pted->p.pted_pte32.pte_lo |= PTE_RO_32;

	PMAP_HASH_LOCK(s);
	if ((pte = pmap_ptedinhash(pted)) != NULL) {
		struct pte_32 *ptp32 = pte;

		pte_del(ptp32, va);

		if (PTED_MANAGED(pted)) { /* XXX */
			pmap_attr_save(ptp32->pte_lo & PTE_RPGN_32,
			    ptp32->pte_lo & (PTE_REF_32|PTE_CHG_32));
		}

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp32->pte_lo &= ~(PTE_CHG_32|PTE_PP_32);
		ptp32->pte_lo |= PTE_RO_32;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp32->pte_hi |= PTE_VALID_32;
		sync();		/* Ensure updates completed. */
	}
	PMAP_HASH_UNLOCK(s);
}

/*
 * Lower the protection on the specified physical page.
 *
 * There are only two cases, either the protection is going to 0,
 * or it is going to read-only.
 */
void
pmap_page_protect(struct vm_page *pg, vm_prot_t prot)
{
	struct pte_desc *pted;
	pmap_t pm;

	if (prot == PROT_NONE) {
		while ((pted = LIST_FIRST(&(pg->mdpage.pv_list))) != NULL) {
			pm = pted->pted_pmap;
			PMAP_VP_LOCK(pm);
			pmap_remove_pted(pm, pted);
			PMAP_VP_UNLOCK(pm);
		}
		/* page is being reclaimed, sync icache next use */
		atomic_clearbits_int(&pg->pg_flags, PG_PMAP_EXE);
		return;
	}

	LIST_FOREACH(pted, &(pg->mdpage.pv_list), pted_pv_list)
		pmap_pted_ro(pted, prot);
}

void
pmap_protect(pmap_t pm, vaddr_t sva, vaddr_t eva, vm_prot_t prot)
{
	if (prot & (PROT_READ | PROT_EXEC)) {
		struct pte_desc *pted;

		PMAP_VP_LOCK(pm);
		while (sva < eva) {
			pted = pmap_vp_lookup(pm, sva);
			if (pted && PTED_VALID(pted))
				pmap_pted_ro(pted, prot);
			sva += PAGE_SIZE;
		}
		PMAP_VP_UNLOCK(pm);
		return;
	}
	pmap_remove(pm, sva, eva);
}

/*
 * Restrict given range to physical memory
 */
void
pmap_real_memory(paddr_t *start, vsize_t *size)
{
	struct mem_region *mp;

	for (mp = pmap_mem; mp->size; mp++) {
		if (((*start + *size) > mp->start)
			&& (*start < (mp->start + mp->size)))
		{
			if (*start < mp->start) {
				*size -= mp->start - *start;
				*start = mp->start;
			}
			if ((*start + *size) > (mp->start + mp->size))
				*size = mp->start + mp->size - *start;
			return;
		}
	}
	*size = 0;
}

void
pmap_init()
{
	pool_init(&pmap_pmap_pool, sizeof(struct pmap), 0, 0, 0, "pmap", NULL);
	pool_setlowat(&pmap_pmap_pool, 2);
	pool_init(&pmap_vp_pool, sizeof(struct pmapvp), 0, 0, 0, "vp",
	    &pool_allocator_nointr);
	pool_setlowat(&pmap_vp_pool, 10);
	pool_init(&pmap_pted_pool, sizeof(struct pte_desc), 0, 0, 0, "pted",
	    NULL);
	pool_setlowat(&pmap_pted_pool, 20);

	PMAP_HASH_LOCK_INIT();

	pmap_initialized = 1;
}

void
pmap_proc_iflush(struct proc *p, vaddr_t addr, vsize_t len)
{
	paddr_t pa;
	vsize_t clen;

	while (len > 0) {
		/* add one to always round up to the next page */
		clen = round_page(addr + 1) - addr;
		if (clen > len)
			clen = len;

		if (pmap_extract(p->p_vmspace->vm_map.pmap, addr, &pa)) {
			syncicache((void *)pa, clen);
		}

		len -= clen;
		addr += clen;
	}
}

/* 
 * There are two routines, pte_spill_r and pte_spill_v
 * the _r version only handles kernel faults which are not user
 * accesses. The _v version handles all user faults and kernel copyin/copyout
 * "user" accesses.
 */
int
pte_spill_r(u_int32_t va, u_int32_t msr, u_int32_t dsisr, int exec_fault)
{
	pmap_t pm;
	struct pte_desc *pted;
	struct pte_desc pted_store;

	/* lookup is done physical to prevent faults */

	/* 
	 * This function only handles kernel faults, not supervisor copyins.
	 */
	if (msr & PSL_PR)
		return 0;

	/* if copyin, throw to full excption handler */
	if (VP_SR(va) == PPC_USER_SR)
		return 0;

	pm = pmap_kernel();

	/* 0 - physmaxaddr mapped 1-1 */
	if (va < physmaxaddr) {
		u_int32_t aligned_va;
		vm_prot_t prot = PROT_READ | PROT_WRITE;
		extern caddr_t kernel_text;
		extern caddr_t etext;

		pted = &pted_store;

		if (va >= trunc_page((vaddr_t)&kernel_text) &&
		    va < round_page((vaddr_t)&etext)) {
			prot |= PROT_EXEC;
		}

		aligned_va = trunc_page(va);
		if (ppc_proc_is_64b) {
			pmap_fill_pte64(pm, aligned_va, aligned_va,
			    pted, prot, PMAP_CACHE_WB);
			pte_insert64(pted);
		} else {
			pmap_fill_pte32(pm, aligned_va, aligned_va,
			    pted, prot, PMAP_CACHE_WB);
			pte_insert32(pted);
		}
		return 1;
	}

	return pte_spill_v(pm, va, dsisr, exec_fault);
}

int
pte_spill_v(pmap_t pm, u_int32_t va, u_int32_t dsisr, int exec_fault)
{
	struct pte_desc *pted;
	int inserted = 0;

	/*
	 * If the current mapping is RO and the access was a write
	 * we return 0
	 */
	PMAP_VP_LOCK(pm);
	pted = pmap_vp_lookup(pm, va);
	if (pted == NULL || !PTED_VALID(pted))
		goto out;

	/* Attempted to write a read-only page. */
	if (dsisr & DSISR_STORE) {
		if (ppc_proc_is_64b) {
			if (pted->p.pted_pte64.pte_lo & PTE_RO_64)
				goto out;
		} else {
			if (pted->p.pted_pte32.pte_lo & PTE_RO_32)
				goto out;
		}
	}

	/* Attempted to execute non-executable page. */
	if ((exec_fault != 0) && ((pted->pted_va & PTED_VA_EXEC_M) == 0))
		goto out;

	inserted = 1;
	if (ppc_proc_is_64b)
		pte_insert64(pted);
	else
		pte_insert32(pted);

out:
	PMAP_VP_UNLOCK(pm);
	return (inserted);
}


/*
 * should pte_insert code avoid wired mappings?
 * is the stack safe?
 * is the pted safe? (physical)
 * -ugh
 */
void
pte_insert64(struct pte_desc *pted)
{
	struct pte_64 *ptp64;
	int off, secondary;
	int sr, idx, i;
	void *pte;
	int s;

	PMAP_HASH_LOCK(s);
	if ((pte = pmap_ptedinhash(pted)) != NULL)
		pte_zap(pte, pted);

	pted->pted_va &= ~(PTED_VA_HID_M|PTED_VA_PTEGIDX_M);

	sr = ptesr(pted->pted_pmap->pm_sr, pted->pted_va);
	idx = pteidx(sr, pted->pted_va);

	/*
	 * instead of starting at the beginning of each pteg,
	 * the code should pick a random location with in the primary
	 * then search all of the entries, then if not yet found,
	 * do the same for the secondary.
	 * this would reduce the frontloading of the pteg.
	 */

	/* first just try fill of primary hash */
	ptp64 = pmap_ptable64 + (idx) * 8;
	for (i = 0; i < 8; i++) {
		if (ptp64[i].pte_hi & PTE_VALID_64)
			continue;

		pted->pted_va |= i;

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp64[i].pte_hi = pted->p.pted_pte64.pte_hi & ~PTE_VALID_64;
		ptp64[i].pte_lo = pted->p.pted_pte64.pte_lo;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp64[i].pte_hi |= PTE_VALID_64;
		sync();		/* Ensure updates completed. */

		goto out;
	}

	/* try fill of secondary hash */
	ptp64 = pmap_ptable64 + (idx ^ pmap_ptab_mask) * 8;
	for (i = 0; i < 8; i++) {
		if (ptp64[i].pte_hi & PTE_VALID_64)
			continue;

		pted->pted_va |= (i | PTED_VA_HID_M);

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp64[i].pte_hi = pted->p.pted_pte64.pte_hi & ~PTE_VALID_64;
		ptp64[i].pte_lo = pted->p.pted_pte64.pte_lo;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp64[i].pte_hi |= (PTE_HID_64|PTE_VALID_64);
		sync();		/* Ensure updates completed. */

		goto out;
	}

	/* need decent replacement algorithm */
	off = ppc_mftb();
	secondary = off & 8;


	pted->pted_va |= off & (PTED_VA_PTEGIDX_M|PTED_VA_HID_M);

	idx = (idx ^ (PTED_HID(pted) ? pmap_ptab_mask : 0));

	ptp64 = pmap_ptable64 + (idx * 8);
	ptp64 += PTED_PTEGIDX(pted); /* increment by index into pteg */

	if (ptp64->pte_hi & PTE_VALID_64) {
		vaddr_t va;

		/* Bits 9-19 */
		idx = (idx ^ ((ptp64->pte_hi & PTE_HID_64) ?
		    pmap_ptab_mask : 0));
		va = (ptp64->pte_hi >> PTE_VSID_SHIFT_64) ^ idx;
		va <<= ADDR_PIDX_SHIFT;
		/* Bits 4-8 */
		va |= (ptp64->pte_hi & PTE_API_64) << ADDR_API_SHIFT_32;
		/* Bits 0-3 */
		va |= (ptp64->pte_hi >> PTE_VSID_SHIFT_64)
		    << ADDR_SR_SHIFT;

		pte_del(ptp64, va);

		pmap_attr_save(ptp64->pte_lo & PTE_RPGN_64,
		    ptp64->pte_lo & (PTE_REF_64|PTE_CHG_64));
	}

	/* Add a Page Table Entry, section 7.6.3.1. */
	ptp64->pte_hi = pted->p.pted_pte64.pte_hi & ~PTE_VALID_64;
	if (secondary)
		ptp64->pte_hi |= PTE_HID_64;
	ptp64->pte_lo = pted->p.pted_pte64.pte_lo;
	eieio();	/* Order 1st PTE update before 2nd. */
	ptp64->pte_hi |= PTE_VALID_64;
	sync();		/* Ensure updates completed. */

out:
	PMAP_HASH_UNLOCK(s);
}

void
pte_insert32(struct pte_desc *pted)
{
	struct pte_32 *ptp32;
	int off, secondary;
	int sr, idx, i;
	void *pte;
	int s;

	PMAP_HASH_LOCK(s);
	if ((pte = pmap_ptedinhash(pted)) != NULL)
		pte_zap(pte, pted);

	pted->pted_va &= ~(PTED_VA_HID_M|PTED_VA_PTEGIDX_M);

	sr = ptesr(pted->pted_pmap->pm_sr, pted->pted_va);
	idx = pteidx(sr, pted->pted_va);

	/*
	 * instead of starting at the beginning of each pteg,
	 * the code should pick a random location with in the primary
	 * then search all of the entries, then if not yet found,
	 * do the same for the secondary.
	 * this would reduce the frontloading of the pteg.
	 */

	/* first just try fill of primary hash */
	ptp32 = pmap_ptable32 + (idx) * 8;
	for (i = 0; i < 8; i++) {
		if (ptp32[i].pte_hi & PTE_VALID_32)
			continue;

		pted->pted_va |= i;

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp32[i].pte_hi = pted->p.pted_pte32.pte_hi & ~PTE_VALID_32;
		ptp32[i].pte_lo = pted->p.pted_pte32.pte_lo;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp32[i].pte_hi |= PTE_VALID_32;
		sync();		/* Ensure updates completed. */

		goto out;
	}

	/* try fill of secondary hash */
	ptp32 = pmap_ptable32 + (idx ^ pmap_ptab_mask) * 8;
	for (i = 0; i < 8; i++) {
		if (ptp32[i].pte_hi & PTE_VALID_32)
			continue;

		pted->pted_va |= (i | PTED_VA_HID_M);

		/* Add a Page Table Entry, section 7.6.3.1. */
		ptp32[i].pte_hi = pted->p.pted_pte32.pte_hi & ~PTE_VALID_32;
		ptp32[i].pte_lo = pted->p.pted_pte32.pte_lo;
		eieio();	/* Order 1st PTE update before 2nd. */
		ptp32[i].pte_hi |= (PTE_HID_32|PTE_VALID_32);
		sync();		/* Ensure updates completed. */

		goto out;
	}

	/* need decent replacement algorithm */
	off = ppc_mftb();
	secondary = off & 8;

	pted->pted_va |= off & (PTED_VA_PTEGIDX_M|PTED_VA_HID_M);

	idx = (idx ^ (PTED_HID(pted) ? pmap_ptab_mask : 0));

	ptp32 = pmap_ptable32 + (idx * 8);
	ptp32 += PTED_PTEGIDX(pted); /* increment by index into pteg */

	if (ptp32->pte_hi & PTE_VALID_32) {
		vaddr_t va;

		va = ((ptp32->pte_hi & PTE_API_32) << ADDR_API_SHIFT_32) |
		     ((((ptp32->pte_hi >> PTE_VSID_SHIFT_32) & SR_VSID)
			^(idx ^ ((ptp32->pte_hi & PTE_HID_32) ? 0x3ff : 0)))
			    & 0x3ff) << PAGE_SHIFT;

		pte_del(ptp32, va);

		pmap_attr_save(ptp32->pte_lo & PTE_RPGN_32,
		    ptp32->pte_lo & (PTE_REF_32|PTE_CHG_32));
	}

	/* Add a Page Table Entry, section 7.6.3.1. */
	ptp32->pte_hi = pted->p.pted_pte32.pte_hi & ~PTE_VALID_32;
	if (secondary)
		ptp32->pte_hi |= PTE_HID_32;
	ptp32->pte_lo = pted->p.pted_pte32.pte_lo;
	eieio();	/* Order 1st PTE update before 2nd. */
	ptp32->pte_hi |= PTE_VALID_32;
	sync();		/* Ensure updates completed. */

out:
	PMAP_HASH_UNLOCK(s);
}
