/*
 * Copyright (C) 1996-1998 by the Board of Trustees
 *    of Leland Stanford Junior University.
 * 
 * This file is part of the SimOS distribution. 
 * See LICENSE file for terms of the license. 
 *
 */

/****************************************************************
 * r4k_cp0.c
 * 
 * This file contains the R4000 implementation of the TLB.  For
 * further elucidation see r4k_cp0.c in Mipsy.
 * Author: $Author: blythe $
 * Date:   $Date: 2002/05/29 01:09:10 $
 *****************************************************************/
#include <bstring.h>
#include "list.h"
#include "simmisc.h"
#include "embra.h"
#include "cp0.h"
#include "main_run.h"
#include "mem_control.h"
#include "qc.h"
#include "clock.h"
#include "decoder.h"
#include "driver.h"
#include "eventcallback.h"
#include "simmagic.h"

#include "stats.h"
#include "annotations.h"
#include "firewall.h"
#include "hw_events.h"
#include "registry.h"
#include "c_port.h"

#include "machine_defs.h"
#include "tc.h"

int embraprintsr = 0;
int exceptionDuringBackdoor = FALSE;

#define REFILL_FLAG 0x8000000
#define XREFILL_FLAG 0x4000000

/*
 * MIPS r4k manual is WRONG on this one.
 * Always latch badvaddr
 */

#define LATCH_BADVADDR(_sr) (1)


#if defined(SIM_MIPS32)
#define ANY_HIGH32_BITS(_vAddr) (0)
#define HAS_BAD_VADDR_BITS(_vAddr) (0)
#define REGION_COMPARE(_a,_b) (1)
#define IN_32BIT_MODE(_p) (1)
#define ONLY_4K_PAGE_SIZE   /* 32bit only needs 4K pages */
#define IS_R10000(P) 0
#else

#define ANY_HIGH32_BITS(_vAddr) ((_vAddr) >> 32)
#define _BAD_VADDR_MASK  (~(((0x1LL << VA_VALID_BITS)-1)|(0x3LL<<62)))
#define HAS_BAD_VADDR_BITS(_vAddr) ((_vAddr) & _BAD_VADDR_MASK)
#define REGION_COMPARE(_a,_b) ((_a) == (_b))
#define IN_32BIT_MODE(_p) ((_p)->is32bitMode)
/*
 * Is the R10000 TLB?
 */
#define IS_R10000(P) ((P)->numTlbEntries == 64)
#endif

#ifdef ONLY_4K_PAGE_SIZE
#define ComputeTlbEntrySize(_pgmask) (0)
#else
static unsigned char ComputeTlbEntrySize(uint pgMsk);
#endif



struct PgMaskStruct  
PgSz[TLBPGMASK_NUMSIZES] = {
   { ~(TLBPGMASK_4K>>13), (1 << 12), 4*1024-1},
   { ~(TLBPGMASK_16K>>13), (1 << 14), 16*1024-1},
   { ~(TLBPGMASK_64K>>13), (1 << 16), 64*1024-1},
   { ~(TLBPGMASK_256K>>13), (1 << 18), 256*1024-1},
   { ~(TLBPGMASK_1M>>13), (1 << 20), 1024*1024-1},
   { ~(TLBPGMASK_4M>>13), (1 << 22), 4*1024*1024-1},
   { ~(TLBPGMASK_16M>>13), (1 << 24), 16*1024*1024-1},
};

#ifndef ONLY_4K_PAGE_SIZE
#define SZ2MASK(_s) PgSz[(_s)].mask
#define SIZES_TO_CHECK TLBPGMASK_NUMSIZES
#else
#define SZ2MASK(_s) -1
#define SIZES_TO_CHECK 1
#endif


#ifndef ONLY_4K_PAGE_SIZE
static unsigned char ComputeTlbEntrySize(uint pgMsk)
{

  unsigned char s;
  uint match = ~(pgMsk >> 13);

  ASSERT(SIZES_TO_CHECK < (1 << (sizeof(unsigned char)*8)));

  for (s = 0; s < SIZES_TO_CHECK; s++) { 
     if ((uint)SZ2MASK(s) == match) {
        return s;
     }
  }
  CPUError("Bad PgMsk 0x%x in ComputeTlbEntrySize\n", pgMsk);
  return (unsigned char) 0;
}
#endif


/*extern int curr_cpu; */
extern EmbraState* curEmp;

/* Local Data */

/* Local CP0 functions */
static void REFILL_EXCEPTION(int cpuNum, int, int,VA,VA,int);
static uint Do_TLB_Write(int cpuNum, int);
/* This is called to allow insertion into the hash table */
static void Insert_TLB_HT( int cpuNum, int i );
/* for debugging */
void Em_Dump_Tlb(int cpuNum);

static void    UpdateCPUMode(CPUState *P);
static EventCallbackHdr timerCallbackHdr[SIM_MAXCPUS];
static void EmbraSetTimerCallback(int cpuNum);

void Em_Dump_Tlb(int cpuNum)
{
   int i;
   CPUPrint("TLB DUMP %d\n", cpuNum);
   for( i = 0; i < EMP[cpuNum].numTlbEntries; i++) {
      CPUPrint("%d PgMsk:0x%x Hi:0x%llx Lo_0:0x%llx Lo_1:0x%llx\n",
               i,
	       EMP[cpuNum].tlbEntry[i].PgMsk,
               (Reg64)EMP[cpuNum].tlbEntry[i].Hi,
               (Reg64)EMP[cpuNum].tlbEntry[i].Lo0,
	       (Reg64)EMP[cpuNum].tlbEntry[i].Lo1);
   }
}

/* This is called to set up the List_Links hashing chains */
/* This sets up the hash bucket list headers, and the Index list that */
/* they point to */
void Em_Tlb_Init_Lists( int cpuNum )
{
  int i;

  /* If we have been called before, this would have been set up */
  /* Yeah, its a little hacky */
  if( EMP[cpuNum].indexList[10].index == 10 )
	 return;

  for( i = 0; i < TLB_HASH_SIZE; i++ )
	 List_Init( &EMP[cpuNum].tlbIndexHeaders[i] );
  for( i = 0; i < EMP[cpuNum].numTlbEntries; i++ ) {
	 List_InitElement( &EMP[cpuNum].indexList[i].links );
	 EMP[cpuNum].indexList[i].index = i;
	 EMP[cpuNum].indexList[i].onList = 0;
  }
}

void Em_Tlb_Do_Init( int cpuNum, int swtch )
{
   int i;

   Em_Tlb_Init_Lists(cpuNum);
   quick_ASID[cpuNum] = GET_ASID( EMP[cpuNum].CP0[C0_TLBHI] );

   /* This clears qc and sets up K0 */
   qc_renew(cpuNum);

   for (i=0; i < EMP[cpuNum].numTlbEntries; i++) {
      /* Add the new entry(it rejects K0 entries)*/
      /* Don't add to index list if we're switching CPUs */
      if (!swtch)
         Insert_TLB_HT( cpuNum, i );
      /* XXX Initialize entry size!!! Should this be done here?? -BL */
      EMP[cpuNum].tlbEntrySize[i] = ComputeTlbEntrySize( EMP[cpuNum].tlbEntry[i].PgMsk);
      /* Update the QC only with current ASID*/
      if( (IS_GLOBAL_HI( EMP[cpuNum].tlbEntry[i].Hi ) ||
           quick_ASID[cpuNum] == GET_ASID( EMP[cpuNum].tlbEntry[i].Hi ) ) )
         {
            qc_map_page( cpuNum,i);
         }
   }
   UpdateCPUMode(&EMP[cpuNum]);
}

/*********************************************************************
 * This can only be called AFTER Tlb_Init is called 
 * This is really just a qc_mmu_context_switch, but our assumptions 
 * are different.  Namely, 1. the current quick_ASID can be 0and 
 * 2. We need to map global entries in the QC
 *********************************************************************/
void Em_Tlb_Init( int cpuNum, int swtch )
{
  int i;
  static int mmu_initialized = 0;

  /* Note: we arrive here when either:
   *  - SimOS starts in Embra mode
   *  - we switch from Mipsy to Embra for the first time
   *  - we switch from Mipsy to Embra after already having done so before.
   * In the first two cases, the mmu has not been allocated/initialized
   * yet, so we don't want to do the Tlb_Clear.
   */
  if (mmu_initialized++)
    Em_Tlb_Clear( cpuNum );

  if( embra.MPinUP ) {
     int cpu;
     for( cpu = 0; cpu < TOTAL_CPUS; cpu++) {
        Em_Tlb_Do_Init( cpu, swtch );
     }
  } else {
     Em_Tlb_Do_Init( cpuNum, swtch );
  }
}


static void  Em_Tlb_Remove(int cpuNum,int idx)
{

  if( embra.emode == EMBRA_CACHE ) {
    qc_cache_inval_page( cpuNum, idx);
  }
  qc_tlb_replace_page( cpuNum, idx);
}

/* We call this on exit from mshade */
void Em_Tlb_Clear( int cpuNum )
{
  int i;
  /* If we have been called before, this would have been set up */
  /* Yeah, its a little hacky */
  if( EMP[cpuNum].indexList[10].index != 10 )
	 return;

  if(embra.MPinUP ) {
    int cpu;
    ASSERT( cpuNum==0);
    for( cpu = 0; cpu < TOTAL_CPUS; cpu++ ) {
      for( i = 0; i < EMP[cpuNum].numTlbEntries; i++ ) {
	if( EMP[cpu].indexList[i].onList ) {
	  List_Remove( &EMP[cpu].indexList[i].links );
	  EMP[cpu].indexList[i].onList = 0;
	  Em_Tlb_Remove(cpu,i);
	}
      }
    }
  } else {
    for( i = 0; i < EMP[cpuNum].numTlbEntries; i++ ) {
      if( EMP[cpuNum].indexList[i].onList ) {
	List_Remove( &EMP[cpuNum].indexList[i].links );
	EMP[cpuNum].indexList[i].onList = 0;
	Em_Tlb_Remove(cpuNum,i);
      }
    }
  }
}

#if defined(SIM_MIPS32)
#define TLBHash(_vpn2,_region, _asid) TLBHash32BitOnly(_vpn2,_asid)


/*****************************************************************
 * Hash function for converting a virtual page number and ASID
 * into an entry in the hash table.  Hash function of R3000 ==
 * hash function of R4000.  Hash table stores chained TLB entry 
 * numbers at each bucket, thus decreasing translation time.
 * R4000 COMMENTS >> In all calls to this function, the vpn is 
 * actually vpn2. (you want both vpn's to map to the same index!)
 *****************************************************************/
static int 
TLBHash32BitOnly(register VPN vpn2, register ASID asid)
{
   return ((vpn2 ^ (vpn2 >> (NUM_VPN_BITS - LOG_TLB_HASH_SIZE)) 
            ^ (asid << (LOG_TLB_HASH_SIZE - NUM_ASID_BITS))) 
           % TLB_HASH_SIZE);
}

#else
#define TLBHash(_vpn2,_region, _asid) TLBHash64bit(_vpn2,_region, _asid)

/*****************************************************************
 * Hash function for converting a virtual page number and ASID
 * into an entry in the hash table.  
 * Hash table stores chained TLB entry 
 * numbers at each bucket, thus decreasing translation time.
 *****************************************************************/
static int 
TLBHash64bit(register VPN vpn2, register uint region, register ASID asid)
{
   return ((vpn2 ^ (vpn2 >> (NUM_VPN2_BITS - LOG_TLB_HASH_SIZE)) 
            ^ (vpn2 >> (32 - (NUM_OFFSET_BITS+1) - LOG_TLB_HASH_SIZE)) 
            ^ region
            ^ (asid << (LOG_TLB_HASH_SIZE - NUM_ASID_BITS))) 
           % TLB_HASH_SIZE);
}
#endif

/*************************************************************************
 * This function returns the index+1 of the TLB entry with the given 
 * vpn2 (!! takes in vpn2, not vpn), and given asid.  It returns 0 if 
 * the index is not found.  
 * The algorithm is: probe the hash table under the given asid, then probe 
 * under asid 0 (to catch global entries ) 
*************************************************************************/
IDX Tlb_Lookup( int cpuNum, uint region, VPN vpn2, ASID asid )
{
  int hashNum;
  List_Links* indexPtr;
  register unsigned idx;
  register int s;
  register EmbraState *P = EMP + cpuNum;

  for (s = 0; s < SIZES_TO_CHECK; s++) { 
     VPN vpn;
     VPN vpnLookup = vpn2 & SZ2MASK(s);
     /* Check for pages under this process's true asid */
     hashNum = TLBHash(vpnLookup, region, asid);
  
     LIST_FORALL( &(P->tlbIndexHeaders[hashNum]), indexPtr) {

        idx = ((IndexListLink*)indexPtr)->index;
        vpn = vpn2 & SZ2MASK(P->tlbEntrySize[idx]);
        /* Don't check global bit because if the global bit is set, the 
         * entry is stored under ASID 0 */
        if ((vpn == GET_VPN2(P->tlbEntry[idx].Hi))
            && (asid == GET_ASID(P->tlbEntry[idx].Hi))
            && REGION_COMPARE(region, GET_REGION(P->tlbEntry[idx].Hi))) {
           return idx+1;
        }
     }
  
     /* Now check for global pages, all of which are stored under asid 0 */
     hashNum = TLBHash(vpnLookup, region, 0);
     LIST_FORALL( &(P->tlbIndexHeaders[hashNum]), indexPtr) {
        idx = ((IndexListLink*)indexPtr)->index;
        vpn = vpn2 & SZ2MASK(P->tlbEntrySize[idx]);
        if ((vpn == GET_VPN2(P->tlbEntry[idx].Hi))
            && (IS_GLOBAL_HI(P->tlbEntry[idx].Hi))
            && REGION_COMPARE(region,GET_REGION(P->tlbEntry[idx].Hi))) {
           return idx+1;
        }
     }
  }
  return 0;
}

/*****************************************************************
 * Address translation on R4000.
 *
 * input: virtual address
 * pointer to phys addr
 * Fully associative search (using hash table)
 * 1.) User Mode or Supervisor Mode
 *     if MSB == 1 in user or MSB = 0 | 110 
 * 2.) VPN2 match?
 *     a.) if VPN2 does not match and 32 bit-mode, -> TLB Refill.
 *     b.) VPN2 match, Not Global, No ASID match, -> TLB Refill.
 *     c.) VPN2 match, not Global and ASID match, OR Global, then if not valid
 *     -> TLB Invalid.
 *     d.) valid, not dirty, but writing -> TLB Mod.
 *     e.) valid, dirty, or not dirty and not writing, Non_Cacheable == T
 *     then -> Access Main Mem, else Access Cache.  Phys addr output.    
 *
 * Returns FAILURE if an exception occurred telling the CPU to try
 * this instruction again. 
 * 
 * 
 * This doesn't really return EXCEPTION_CODE, because the EXCEPTION
 * routine does not return, it jumps to continue_run with the 
 * handler PC.  I keep them in to be descriptive.
 *
 *****************************************************************/
uint
Em_TranslateVirtual(int cpuNum, VA vAddr, PA *pAddr, Em_accesstype act )
{
  IDX         tlbIndex;
  ContextReg contextReg;
  XContextReg xcontextReg;
  int myASID = GET_ASID(curEmp->CP0[C0_TLBHI]);
  Reg         VPN2;
  Reg        lo_reg;
  int           region = GET_REGION(vAddr);
  unsigned char writing = (act == ACT_DWRITE);
  Reg32      sr_reg = (Reg32)(curEmp->CP0[C0_SR]);

  ASSERT( curEmp->myNum == cpuNum);

  /* Not the best way to do this... */
  if( !curEmp->outOfSlaveLoop ) {
     return BACKDOOR_CODE;
  }


   if (region == 0) { 
      /* Make sure the address is with acceptable range. For the
       * 32 bit mode this means 32 bits, for 64bit mode this means
       * impelementation defined bits. 
       */
      if (IN_32BIT_MODE(curEmp)) {
         if (ANY_HIGH32_BITS(vAddr)) goto addrErr;
      } else {
         if (HAS_BAD_VADDR_BITS(vAddr)) goto addrErr;
         if (IS_R10000(curEmp) && !(sr_reg & SR_UX) && ANY_HIGH32_BITS(vAddr)) goto addrErr;
      }
      /* KUSEG becomes an uncached space when the ERL bit is set. This is 
       * needed for cache error handling. */
      if (sr_reg & SR_ERL) { 
         /* need to add k1base because registry works on kseg1 addresses */
         vAddr += K1BASE;
         goto bdoor;
      }
      /* Fall thru to TLB lookup */

   } else if (region == 3) {
      /* Kernel region, no user and limited supervisor */
      if (vAddr >= CKSEG0_START_ADDR) {
         if (!IS_KERNEL_MODE(curEmp)) {
            /* No user and supervisor limited to single range */
            if (IS_BACKDOOR(vAddr) ) goto bdoor;
            if (sr_reg & SR_KSU_USR) goto addrErr;
            if ((sr_reg & SR_KSU_SUP) && !IS_SUPERV_SEG(vAddr)) goto addrErr;
         }
         if (IS_KSEG0(vAddr)) {
            if (!IS_KERNEL_MODE(curEmp)) goto addrErr; /* Kernel only */
            if (remapVec->RemapEnable[cpuNum] &&
              (vAddr >= __MAGIC_OSPC_BASE && vAddr < __MAGIC_OSPC_END))
               goto bdoor;
            *pAddr = K0_TO_PHYS_REMAP(vAddr, cpuNum);
            if (!IS_VALID_PA(M_FROM_CPU(cpuNum), *pAddr))  goto addrErr;
            if (CPUVec.CheckFirewall)
               goto firewall;
            return NORMAL_CODE;
         }
         if (IS_KSEG1(vAddr)) goto bdoor;
         /* Fall thru to TLB lookup */
      } else { 
         /* If we got here we better not be in 32bit mode and
          * there is nothing that the user or supvisor can access.
          */
         if (IN_32BIT_MODE(curEmp) || !IS_KERNEL_MODE(curEmp)) goto addrErr;
         if (vAddr > XKSEG_END_ADDR) goto addrErr;
      }
      /* Fall thru to TLB lookup */
#if defined(SIM_MIPS64)
   } else if (region == 2) {
      uint cache_algorithm = XKPHYS_CACHE_ALGORITHM(vAddr);
      /* xkphys - only available in kernel 64bit mode */
      if (IN_32BIT_MODE(curEmp) || !IS_KERNEL_MODE(curEmp)) goto addrErr;

      switch (cache_algorithm) {
      case CBIT_UPDATE:
         if (IS_R10000(curEmp)) {
            break; /* fall thru to error, doesn't work on R10000 */
         }
      case CBIT_NONCOHERENT:
      case CBIT_EXCLUSIVE:
      case CBIT_EXCLUSIVE_WRITE:
      {
         Reg64 offset = XKPHYS_ONE_PAGE_OFFSET(vAddr);
         if (XKPHYS_INVALID_OFFSET(offset)) goto addrErr;
         *pAddr = K0_TO_PHYS_REMAP(XKPHYS_BASE + offset, cpuNum);
         /* XXX - NEED TO ACCESS FLAVOR */
         return NORMAL_CODE;
      }
      case CBIT_UNCACHED:
      case CBIT_UNCACHED_ACCEL:
      {
         uint flavor = XKPHYS_UNCACHED_FLAVOR(vAddr);
         Reg64 offset = XKPHYS_FOUR_PAGE_OFFSET(vAddr);

         if (XKPHYS_INVALID_OFFSET(offset)) goto addrErr;
         if (!IS_R10000(curEmp) && (flavor != 0)) goto addrErr;
         goto bdoor;
      }
      default:
         /* invalid cache alogrithm fall thru to error */
         break;
      }

      goto addrErr;

   } else if (region == 1) {
      /* Supervisor region - only available in 64bit mode */
      if (IN_32BIT_MODE(curEmp) || ((sr_reg & SR_KSU_USR) && !IS_KERNEL_MODE(curEmp)) || 
          HAS_BAD_VADDR_BITS(vAddr)) goto addrErr;
      if (IS_R10000(curEmp) && !(sr_reg & SR_SX)) goto addrErr;
      /* Fall thru to TLB lookup */
#endif /* notdef MIPS32 */
   }

    /* Check TLB */
    VPN2 = GET_VPN2(vAddr);
    tlbIndex = Tlb_Lookup(cpuNum, region, VPN2, myASID);

    if (tlbIndex) {
      int szEntry; 
      /* We have a matching VPN and ASID - see if it is valid */
      tlbIndex--;
      szEntry = curEmp->tlbEntrySize[tlbIndex];
      /* Which lo register? */
      if (vAddr & PgSz[szEntry].loBit)
         lo_reg = curEmp->tlbEntry[tlbIndex].Lo1;
      else
         lo_reg = curEmp->tlbEntry[tlbIndex].Lo0;
      
      if (IS_VALID(lo_reg)) {
         /* Check if the page is dirty or we are reading */
         if ( IS_DIRTY(lo_reg) || !(writing)) {
            /* Everything is cool - form the address */
#if defined(SIM_MIPS64) && defined(IRIX6_4)
            int cache_algorithm = GET_CACHE_ALGOR(lo_reg);

            if (!((cache_algorithm == CBIT_EXCLUSIVE) || 
                  (cache_algorithm == CBIT_EXCLUSIVE_WRITE))) {
               CPUWarning("%lld Unsupported TLB cache algorithm (%d) at address 0x%llx by cpu %d at PC 0x%llx\n",
                          (uint64)EmbraCpuCycleCount(cpuNum), cache_algorithm, 
                            (uint64)vAddr, cpuNum, (uint64)EMP[cpuNum].PC);

            }
#endif
            *pAddr = (((GET_PFN(lo_reg)&SZ2MASK(szEntry))*4*1024) | 
               (vAddr & PgSz[szEntry].offset_mask));
            if (!EMBRA_IS_PADDR(M_FROM_CPU(cpuNum),*pAddr)) {
              /* Bad physical address */
              if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
              Em_EXCEPTION(cpuNum, EXC_DBE,0);
              return EXCEPTION_CODE;
            }
            if (CPUVec.CheckFirewall)
               goto firewall;            
            return NORMAL_CODE;
         } else { 
            
	    /* TLB MODIFICATION */
	    /* Page is not dirty and we want to write */
	    /* Set up the CTxt register !!! */
	    /* EPC set up in EXCEPTION, badVaddr and context */
	    /* stored in macro */


	    contextReg.tc_data = curEmp->CP0[C0_CTXT];
	    contextReg.s32.tc_badvpn = VPN2;

            xcontextReg.tc_data = curEmp->CP0[C0_XCTXT];
            xcontextReg.s64.tc_region = region;
#ifndef BIG_BIT_FIELD_BROKEN
            xcontextReg.s64.tc_badvpn = VPN2;
#else
            xcontextReg.s64.tc_badvpn_hi3 = VPN2>>28;
            xcontextReg.s64.tc_badvpn_lo28 = VPN2;
#endif
             EMP[cpuNum].CP0[C0_TLBHI] = (((Reg)region << TLBHI_REGIONSHIFT) | 
                     (VPN2 << TLBHI_VPN2SHIFT)  | 
                     ((Reg)myASID << TLBHI_PIDSHIFT)) ;

            if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;

            EMP[cpuNum].CP0[C0_CTXT] = contextReg.tc_data;
            EMP[cpuNum].CP0[C0_XCTXT] = xcontextReg.tc_data;
             
            Em_EXCEPTION(cpuNum,EXC_MOD,0);
            return EXCEPTION_CODE;
         }
      } else { 
	 /* TLB INVALID */

	 contextReg.tc_data = curEmp->CP0[C0_CTXT];
	 contextReg.s32.tc_badvpn = VPN2;

         xcontextReg.tc_data = curEmp->CP0[C0_XCTXT];
         xcontextReg.s64.tc_region = region;
#ifndef BIG_BIT_FIELD_BROKEN
         xcontextReg.s64.tc_badvpn = VPN2;
#else
         xcontextReg.s64.tc_badvpn_hi3 = VPN2>>28;
         xcontextReg.s64.tc_badvpn_lo28 = VPN2;
#endif


         EMP[cpuNum].CP0[C0_TLBHI] =  (((Reg)region << TLBHI_REGIONSHIFT) | 
                     (VPN2 << TLBHI_VPN2SHIFT)  | 
                     ((Reg)myASID << TLBHI_PIDSHIFT)) ;


         EMP[cpuNum].CP0[C0_CTXT] = contextReg.tc_data;
         EMP[cpuNum].CP0[C0_XCTXT] = xcontextReg.tc_data;
         if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
         Em_EXCEPTION(cpuNum,writing ? EXC_WMISS : EXC_RMISS,0);
         return EXCEPTION_CODE;

      }

    }

    /* TLB REFILL 
     * Since there were no matching VPN2s, there is a TLB refill exception.
     * First put the VPN2 and ASID of the non-matching address in Hi.
     * The BadVAddr and Context registers also need to be set.
         * 1.) set TLBL or TLBS(store only) code in cause register
     * use EPC and BD bit in cause reg, inst or load, or store.
     * 2.) BadVAddr, Context, XContect and EntryHi hold the vAddr
     * that failed. Entry HI also has ASID. EPC pts to last instruction
     * take care of branches.
     */



    contextReg.tc_data = curEmp->CP0[C0_CTXT];
    contextReg.s32.tc_badvpn = VPN2;

    xcontextReg.tc_data = curEmp->CP0[C0_XCTXT];
    xcontextReg.s64.tc_region = region;
#ifndef BIG_BIT_FIELD_BROKEN
    xcontextReg.s64.tc_badvpn = VPN2;
#else
    xcontextReg.s64.tc_badvpn_hi3 = VPN2>>28;
    xcontextReg.s64.tc_badvpn_lo28 = VPN2;
#endif

    /* BadVAddr register should be loaded */
    if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;

    EMP[cpuNum].CP0[C0_TLBHI] =   (((Reg)region << TLBHI_REGIONSHIFT) | 
                     (VPN2 << TLBHI_VPN2SHIFT)  | 
                     ((Reg)myASID << TLBHI_PIDSHIFT)) ;

    EMP[cpuNum].CP0[C0_CTXT] = contextReg.tc_data;
    EMP[cpuNum].CP0[C0_XCTXT] = xcontextReg.tc_data;

    if (sr_reg & SR_EXL) { 
       Em_EXCEPTION(cpuNum,writing ? EXC_WMISS : EXC_RMISS, 0);

    } else {    
       int isXRefill = 0;

       if (((region == 0) && (sr_reg & SR_UX)) ||
           ((region == 3) && (sr_reg & SR_KX)) ||
           ((region == 1) && (sr_reg & SR_SX))) {
          isXRefill = 1;
       }
       REFILL_EXCEPTION(cpuNum,writing ? EXC_WMISS : EXC_RMISS, 
                        act== ACT_IREAD,
                        EMP[cpuNum].PC,
                        vAddr, isXRefill);
    }

  return EXCEPTION_CODE;


addrErr:
    /* Illegal address - generate an address error */

  if (LATCH_BADVADDR(sr_reg)) curEmp->CP0[C0_BADVADDR] = vAddr;
  Em_EXCEPTION(cpuNum, (act==ACT_IREAD) ? EXC_IBE : EXC_DBE, 0);
  return EXCEPTION_CODE;                                   


bdoor:
   {
      void *dat;
      uint flag;
      int  rval;

      if (vAddr == EMP[cpuNum].PC) {
         *pAddr = (PA)RegistryGetSimFunction(vAddr);
         if (*pAddr) return BACKDOOR_CODE;
         /* no such function */
         {
            /* better only warn once because cpu can go into
             * an infinite loop: the IBE handler is a bad
             * backdoor address if BEV is set
             */
            static int warned = 0;
            if (!warned) {
               CPUWarning("%lld Bad backdoor ifetch to address 0x%x by cpu %d\n",
                          (uint64)EmbraCpuCycleCount(cpuNum), vAddr, cpuNum);
               warned = 1;
               CPUWarning("Bad backdoor warning will not be repeated\n");
            }
         }
         if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
         Em_EXCEPTION(cpuNum, EXC_IBE, 0);
         return EXCEPTION_CODE;
      }

      if (!RegistryIsInRange(vAddr,&dat, &flag)) {
         CPUWarning("%lld Bad backdoor reference to address 0x%x by cpu %d\n",
                    (uint64)EmbraCpuCycleCount(cpuNum), vAddr, cpuNum);
         if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
         Em_EXCEPTION(cpuNum, 
			(act==ACT_IREAD) ? EXC_IBE : EXC_DBE, 
			0);
         return EXCEPTION_CODE;
      }
      if (flag & REG_DATA) {
         *pAddr = (PA) dat;
      } else {
         Instruction instr;
         Result returnFlag;
	 /* NOTE: the following MUST be static, since for backdoor loads,
	  * the value "loaded" from the backdoor is returned here (actually,
	  * pAddr is set to point to this, and the caller fetches the
	  * value, foolishly believing it's accessing mem.
	  * This is a hack and should be fixed (yeah, when?).
	  */
         static uint64 buffer;

         /* data addressed backdoor function: decode the simulated instruction to 
            find out the register number, and pass that value to backdoor function.
            Only works for KSEG0 PCs, otherwise would have to translate the PC. */

         ASSERT(flag & REG_FUNC);
         if (!IS_KERNEL_MODE(&EMP[cpuNum])) {
            CPUError("BACKDOOR ACCESS WHILE NOT IN KERNEL: pc=%#llx ra=%#llx addr=%#llx \n",
                     (uint64)curEmp->PC,(uint64)curEmp->R[31],(uint64)vAddr);
         }
	 /*
	  * XXX NOTE: is this the right place to update the current CPU??
	  * I'm not sure, but this should work (with little perf impact),
	  * at least for the backdoor.
	  */

	 /* caveat: we might be in a branch delay slot... */
#ifdef EMBRA_USE_QC64
         instr = *(Instruction *)Em_QC64Reload(CLEAR_BD(EMP[cpuNum].PC),
                                               QC64_READ);
#else
         instr = *(Instruction *)K0_TO_MEMADDR(M_FROM_CPU(cpuNum),
                                               CLEAR_BD(EMP[cpuNum].PC));
#endif
         
         switch ( MAJOR_OPCODE(instr) ) {
         case lb_op:
	 case lbu_op:
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_LOAD_BYTE, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
         case lh_op:
	 case lhu_op:
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_LOAD_HALF, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
         case lw_op:
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_LOAD_WORD, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
         case ld_op: /* XXX backward compatibility XXX */
	 case ldc2_op:
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_LOAD_DOUBLE, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
         case sb_op:

	    ((unsigned char *)&buffer)[0] = 0xff & EMP[cpuNum].R[rt(instr)];
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_STORE_BYTE, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
	    break;
         case sh_op:
	    ((unsigned short *)&buffer)[0] = 0xffff & EMP[cpuNum].R[rt(instr)];
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_STORE_HALF, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
	    break;
         case sw_op:
	    ((uint *)&buffer)[0] = EMP[cpuNum].R[rt(instr)];
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_STORE_WORD, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
	    break;
#if defined(SIM_MIPS64)
         case sd_op: 
            ((Reg64 *)&buffer)[0] = EMP[cpuNum].R[rt(instr)];
	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_STORE_DOUBLE, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
#else
         case sd_op: /* XXX backward compatibility XXX */
#endif
	 case sdc2_op:
            ((uint *)&buffer)[0] = EMP[cpuNum].R[rt(instr)];
            ((uint *)&buffer)[1] = EMP[cpuNum].R[rt(instr)+1];

	    rval = ((MagicFunction)dat)(cpuNum,
					vAddr, BDOOR_STORE_DOUBLE, &buffer);
	    returnFlag = rval ? BUSERROR : SUCCESS;
            break;
         default:
            CPUError("Data addressed functions are only triggered from [ls][bhwd]\n");
         }

         if (returnFlag == SUCCESS) {
            *pAddr = (PA) &buffer;
         } else if (returnFlag == BUSERROR) {
            *pAddr = NULL;
            if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
            Em_EXCEPTION(cpuNum, EXC_DBE, 0);
            return EXCEPTION_CODE;
         } else {
            CPUError("Unknown result type: 0x%x\n", returnFlag);
         }
      }
   }
#ifdef NOTDEF
  /* Log all backdoor writes since you're totally paranoid now */
  LogEntry("BACKDOOR", cpuNum, "0x%x 0x%x\n", vAddr, *pAddr);
#endif
  return  BACKDOOR_CODE;

firewall:
  {
     /* first check if line became incoherent during low-level recovery */
     if (SimMagic_IsIncoherent(*pAddr)) {
            if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
        Em_EXCEPTION(cpuNum, (act==ACT_IREAD) ? EXC_IBE : EXC_DBE, 0);
        return EXCEPTION_CODE;
     }

     /* come here if access is fine but firewall needs to be checked */
     if (writing && !CPUVec.CheckFirewall(cpuNum, *pAddr)) {
        if (LATCH_BADVADDR(sr_reg)) EMP[cpuNum].CP0[C0_BADVADDR] = vAddr;
        Em_EXCEPTION(cpuNum, EXC_DBE, 0);
        CPUWarning("Fwall violation: "
                   "cpu %d vaddr 0x%x paddr 0x%x\n",
                   cpuNum, vAddr, *pAddr);
        return EXCEPTION_CODE;
     }

/*if qc changes because of R4000 this might need changes!!*/
     /* if address is k0seg, then add phys->mem mapping to mmu so we 
	don't have to translate again.  */

     /* DISABLED: we now preemptively put all the k0seg translations
      * in in qc_renew, then zero out the ones with annotations
      * in EmbraInstallMemAnnotation.  So we don't need to worry
      * about putting more here (and could disable annotations if
      * we accidentally did so).
      */

#ifdef notdef
     if ((vAddr > K0BASE && vAddr < K0BASE + K0SIZE) &&
	 (EMP[cpuNum].mmu[PAGE_NUMBER(vAddr)] == 0)) {
         PA pa = (PA)PHYS_TO_MEMADDR(M_FROM_CPU(cpuNum),
                                     FORM_ADDR(PAGE_NUMBER(*pAddr), 0));
         EMP[cpuNum].mmu[PAGE_NUMBER(vAddr)] = 
             (embra.emode == EMBRA_PAGE && CheckFirewall(cpuNum, *pAddr)) ?
             MMU_PROT_WRITE(pa) : MMU_PROT_READ(pa);
     }
#endif

     return NORMAL_CODE;
  }
}



/*****************************************************************
 * Is_Tlb_Writable -
 * See if the given VPN, asid pair holds a writable (dirty bit is set)
 * mapping in the TLB.
 *****************************************************************/
int Em_Is_Tlb_Writable( int cpuNum, VA vAddr, ASID asid )
{
   int tlb_index;
   if( IS_KSEG0(vAddr) ) /* KSEG1 also unmapped */
      return 1;
   /* Check the TLB Hash Table */
   tlb_index = Tlb_Lookup( cpuNum, GET_REGION(vAddr), CONVERT_TO_VPN2(PAGE_NUMBER(vAddr)),
			   asid );
   
   if( tlb_index ) {
     tlb_index--;
     if (IS_LO_0(PAGE_NUMBER(vAddr)))
       return( IS_DIRTY(EMP[cpuNum].tlbEntry[tlb_index].Lo0));
     else
       return( IS_DIRTY(EMP[cpuNum].tlbEntry[tlb_index].Lo1));
   }
   return 0;
}

/*****************************************************************
 * Is_Tlb_Readable -
 * See if the given VPN, asid pair holds a valid
 * mapping in the TLB.
 *****************************************************************/
int Em_Is_Tlb_Readable( int cpuNum, VA vAddr, ASID asid )
{
   int tlb_index;
   if( IS_KSEG0(vAddr) )
      return 1;
   /* Check the TLB Hash Table */
   tlb_index = Tlb_Lookup( cpuNum, GET_REGION(vAddr), 
                           CONVERT_TO_VPN2(PAGE_NUMBER(vAddr)), asid );

   if( tlb_index ) {
     tlb_index--;
     if (IS_LO_0(PAGE_NUMBER(vAddr)))
       return( IS_VALID(EMP[cpuNum].tlbEntry[tlb_index].Lo0));
     else
       return( IS_VALID(EMP[cpuNum].tlbEntry[tlb_index].Lo1));
   }
   return 0;
}
  
/*****************************************************************
 * ProbeTLB -
 * Probe TLB for a matching entry. The Index register is loaded 
 * with the address of the TLB entry whose contents match the 
 * contents of the EntryHi register. If no TLB entry matches, the 
 * high order bit of the Index register is set. 
 * 
 * Multiple matches are not possible (they would have caused a TLB 
 * shutdown error
 *****************************************************************/
uint Em_ProbeTLB(int cpuNum)
{
  int tlb_idx;
  ASSERT (curEmp->myNum == cpuNum);


  if (!IS_KERNEL_MODE(curEmp)) { 
     EMP[cpuNum].CP0[C0_CAUSE] = CAUSE_SET_CE( EMP[cpuNum].CP0[C0_CAUSE], 0 );
     Em_EXCEPTION(cpuNum,EXC_CPU,0);	/* Coprocessor Unusable */
     ReenterTC(curEmp);
  }
  
  tlb_idx = Tlb_Lookup( cpuNum, GET_REGION( EMP[cpuNum].CP0[C0_TLBHI]),
                        GET_VPN2( EMP[cpuNum].CP0[C0_TLBHI] ),
                        GET_ASID( EMP[cpuNum].CP0[C0_TLBHI] ) );
  if( tlb_idx ) {
     /* We matched */
     EMP[cpuNum].CP0[C0_INX] = (tlb_idx - 1)<<TLBINX_INXSHIFT;
     return NORMAL_CODE;
  }


  /* maintain inclusion property of mmu */
  /* Since there was no entry with that vpn2 and asid */
  /* we need to take out both pages if present in the mmu */
  /* even page */
  qc_erase_etlb(cpuNum,  (EMP[cpuNum].tlbEntry[tlb_idx-1].Hi));
                           
  /* Probe Miss */
  EMP[cpuNum].CP0[C0_INX] = (Reg32_s)0x80000000;
  return NORMAL_CODE;
}

/*****************************************************************
 * ReadTLBEntry -
 * The EntryHi and EntryLo registers are loaded with the contents 
 * of the TLB entry pointed at by the contents of the TLB Index
 * register. The results are unspecified if the contents or Index
 * are greater than the number of TLB entries.
 *****************************************************************/

uint Em_ReadTLBEntry(int cpuNum)
{
  IndexReg index;
   ASSERT (curEmp->myNum == cpuNum);
 

   if (!IS_KERNEL_MODE(&EMP[cpuNum])) { 
     EMP[cpuNum].CP0[C0_CAUSE] = CAUSE_SET_CE( EMP[cpuNum].CP0[C0_CAUSE], 0 );
     Em_EXCEPTION(cpuNum,EXC_CPU,0);	/* Coprocessor Unusable */
     ReenterTC(curEmp);
     return EXCEPTION_CODE;
  }
   
  /* I'm going to assume that the index register is in the 
	 correct range. */
   
  index = EMP[cpuNum].CP0[C0_INX];
   
  /* Even though its not the intent of the instruction, it */
  /* resets the MMU context */
  qc_mmu_switch( cpuNum, CURRENT_ASID(cpuNum), 
                 GET_ASID(EMP[cpuNum].tlbEntry[GET_IDX(index)].Hi), 0 );

  EMP[cpuNum].CP0[C0_PGMASK] = EMP[cpuNum].tlbEntry[GET_IDX(index)].PgMsk;
#if defined(SIM_MIPS32)
  ASSERT(EMP[cpuNum].tlbEntry[GET_IDX(index)].PgMsk == 0);
#endif
  EMP[cpuNum].CP0[C0_TLBHI] = EMP[cpuNum].tlbEntry[GET_IDX(index)].Hi &
    ~(Reg)EMP[cpuNum].tlbEntry[GET_IDX(index)].PgMsk & ~TLBHI_G;
  EMP[cpuNum].CP0[C0_TLBLO_0] = EMP[cpuNum].tlbEntry[GET_IDX(index)].Lo0;
  EMP[cpuNum].CP0[C0_TLBLO_1] = EMP[cpuNum].tlbEntry[GET_IDX(index)].Lo1;

  if (IS_GLOBAL_HI(EMP[cpuNum].tlbEntry[GET_IDX(index)].Hi)){
    EMP[cpuNum].CP0[C0_TLBLO_0] |= TLBLO_G; 
    EMP[cpuNum].CP0[C0_TLBLO_1] |= TLBLO_G; 
  } else {
    EMP[cpuNum].CP0[C0_TLBLO_0] &= ~TLBLO_G; 
    EMP[cpuNum].CP0[C0_TLBLO_1] &= ~TLBLO_G; 
  }
  
  return NORMAL_CODE;
}

/*****************************************************************
 *  Insert_TLB_HT
 * Insert a TLB entry into the TLB hash table.  This is supposed to be
 *  called after the entry  has been inserted to the TLB_Hi & Lo structures
 *****************************************************************/
static void Insert_TLB_HT( int cpuNum, int idx )
{
  int hashNum;

  /* The hardware matching is independent of the valid bit.  The valid */
  /* bit's sole purpose is to indicate which exception vector to take */
  /* Thus we decide whether to add an entry based on its */
  /* virtual address*/
  if (IS_UNMAPPED_TLBHI(EMP[cpuNum].tlbEntry[idx].Hi))
    return;

  /* Insert all global entries under ASID 0 */
  if( IS_GLOBAL_HI( EMP[cpuNum].tlbEntry[idx].Hi ) )
	{
	  hashNum = TLBHash( GET_VPN2( EMP[cpuNum].tlbEntry[idx].Hi ), 
                             GET_REGION(EMP[cpuNum].tlbEntry[idx].Hi),0 );
	}
  else
	{
	   hashNum = TLBHash( GET_VPN2( EMP[cpuNum].tlbEntry[idx].Hi ),
                               GET_REGION( EMP[cpuNum].tlbEntry[idx].Hi),
			       GET_ASID( EMP[cpuNum].tlbEntry[idx].Hi ) );
#if defined(SIM_MIPS32)
	  if( GET_ASID( EMP[cpuNum].tlbEntry[idx].Hi ) == 0 )
		CPUWarning("Non-global ASID 0 entry written to TLB. %d 0x%x\n",
                           EmbraCpuCycleCount(cpuNum), EMP[cpuNum].PC);
#endif  
	}

  List_Insert( &EMP[cpuNum].indexList[idx].links, 
			   LIST_ATFRONT(&EMP[cpuNum].tlbIndexHeaders[hashNum]) );
  EMP[cpuNum].indexList[idx].onList = 1;
}

/*****************************************************************
 * Do_TLB_Write
 * Write the indexed TLB entry. The TLB entry pointed at by the 
 * contents of the TLB index register is loaded with the contents
 * of the EntryHi and EntryLo registers.
 * Update QC array
 *****************************************************************/
static uint Do_TLB_Write(int cpuNum, int idx )
{
   Reg frameMask;
   
   if (!IS_KERNEL_MODE(&EMP[cpuNum])) { 
      EMP[cpuNum].CP0[C0_CAUSE] = CAUSE_SET_CE( EMP[cpuNum].CP0[C0_CAUSE], 0 );
      Em_EXCEPTION(cpuNum,EXC_CPU,0);	/* Coprocessor Unusable */
      ReenterTC(curEmp);
   }

#ifdef DEBUG_CP0
      CPUPrint("%d TLB Idx:%d  Hi:0x%08x  Lo0:0x%08x Lo1:0x%08x\n",
               cpuNum,
               idx,
               EMP[cpuNum].CP0[C0_TLBHI],
               EMP[cpuNum].CP0[C0_TLBLO_0],
	       EMP[cpuNum].CP0[C0_TLBLO_1] );
#endif
   ASSERT( CURRENT_ASID(cpuNum) == GET_ASID(EMP[cpuNum].CP0[C0_TLBHI]) ||
           GET_ASID(EMP[cpuNum].CP0[C0_TLBHI]) == 0 );

   /* Remove entry from quick check */
   /* It is possible that e.g. (VPN, ASID) (0x1000, 4) is being used, */
   /* and we get a request to remove (0x1000, 2).  Since that enty is */
   /* alreay out of our qc, we don't want to modify it */
   if( GET_ASID( EMP[cpuNum].tlbEntry[idx].Hi ) == CURRENT_ASID(cpuNum) ||
      IS_GLOBAL_HI( EMP[cpuNum].tlbEntry[idx].Hi ) ) {
      /* even page */
      /* Only remove QC state if we are removing an entry that is our */
      /* ASID or is global. This could simply remove the cache info */
     Em_Tlb_Remove(cpuNum,idx);
      /*
       * being anal sometimes pays off. 
       */
#ifndef EMBRA_USE_QC64
      if (embra.emode == EMBRA_CACHE) { 
         uint base = TLBHI2ADDR(EMP[cpuNum].tlbEntry[idx].Hi);
         if (!IS_KSEG0(base)) { 
            int i;
            ASSERT (!(PAGE_NUMBER(base)&1));
            ASSERT (!EMP[cpuNum].mmu[PAGE_NUMBER(base)]);
            ASSERT (!EMP[cpuNum].mmu[PAGE_NUMBER(base)+1]);
            for (i=0; i<2*LINES_PER_PAGE;i++) {
               ASSERT (!EMP[cpuNum].qc_v[ADDR2SLINE(base)+i]);
            }
         }
      }
#endif
   }

   /* Remove old entry from the hash list */
   if( EMP[cpuNum].indexList[idx].onList ) {
      List_Remove( &EMP[cpuNum].indexList[idx].links );
      EMP[cpuNum].indexList[idx].onList = 0;
   }
 
   if (IS_R10000(&EMP[cpuNum])) { 
      frameMask = (EMP[cpuNum].CP0[C0_FRAMEMASK] << TLBFRAMESHIFT);
   } else {
      frameMask = 0;
   }
 
   /* Actually write the TLB data structure */
   EMP[cpuNum].tlbEntry[idx].PgMsk = EMP[cpuNum].CP0[C0_PGMASK];
   EMP[cpuNum].tlbEntry[idx].Hi = EMP[cpuNum].CP0[C0_TLBHI] &
     ~(Reg)EMP[cpuNum].CP0[C0_PGMASK];
   EMP[cpuNum].tlbEntry[idx].Lo0 = 
      (EMP[cpuNum].CP0[C0_TLBLO_0] & ~TLBLO_G) & ~frameMask;
   EMP[cpuNum].tlbEntry[idx].Lo1 = 
      (EMP[cpuNum].CP0[C0_TLBLO_1] & ~TLBLO_G) & ~frameMask;
   EMP[cpuNum].tlbEntrySize[idx] = ComputeTlbEntrySize( EMP[cpuNum].tlbEntry[idx].PgMsk);

   if (IS_GLOBAL_LO(EMP[cpuNum].CP0[C0_TLBLO_0]) && 
       IS_GLOBAL_LO(EMP[cpuNum].CP0[C0_TLBLO_1]))
     EMP[cpuNum].tlbEntry[idx].Hi |= TLBHI_G;

  /* Add the new entry, if necessary  */
  Insert_TLB_HT( cpuNum,idx );
  /* Update the QC only if entry is valid, and either our asid or global*/
  if( GET_ASID( EMP[cpuNum].tlbEntry[idx].Hi ) == CURRENT_ASID(cpuNum)  ||
      IS_GLOBAL_HI( EMP[cpuNum].tlbEntry[idx].Hi))
     {
        qc_map_page( cpuNum, idx);
     }
#ifndef EMBRA_USE_QC64
  qc_CheckForDuplicate(curEmp, idx);
#endif
  return NORMAL_CODE;
}

/*****************************************************************
 * WriteTLBEntry -
 * Write the indexed TLB entry. The TLB entry pointed at by the 
 * contents of the TLB index register is loaded with the contents
 * of the EntryHi and EntryLo registers.
 *****************************************************************/
uint Em_WriteTLBEntry(int cpuNum)
{
  IndexReg index;
  uint res;
  ASSERT (curEmp->myNum == cpuNum);
  if (!IS_KERNEL_MODE(curEmp)) {
     Em_EXCEPTION(cpuNum,EXC_CPU,0);
     ReenterTC(curEmp);
  }

  /* I'm going to assume that the index register is in the 
	 correct range. */   
  index = EMP[cpuNum].CP0[C0_INX];

  res = Do_TLB_Write( cpuNum, GET_IDX(index) );

  if( res != NORMAL_CODE ) {
     ReenterTC(&EMP[cpuNum]);
     /* NOT REACHED */
  }
  return res;
}

/*****************************************************************
 * WriteRandomTLBEntry -
 * Write random TLB entry. The TLB entry pointed at by the contents  
 * of the TLB Random register is loaded with the contents of EntryHi
 * and EntryLo.
 *
 * Technically, the value of the Random register is decremented on
 * each machine clock cycle and ranges between NWIREDENTRIES
 * and NTLBENTRIES. 
 * 
 *****************************************************************/
uint Em_WriteRandomTLBEntry(int cpuNum)
{
   uint res;
   unsigned numRandEntries = EMP[cpuNum].numTlbEntries - EMP[cpuNum].CP0[C0_TLBWIRED];

   ASSERT (curEmp == &EMP[cpuNum]);
   if (!IS_KERNEL_MODE(curEmp)) {
      Em_EXCEPTION(cpuNum,EXC_CPU,0);
      ReenterTC(curEmp);
   }
   
   res =  Do_TLB_Write( cpuNum, 
                        (EmbraCpuCycleCount(cpuNum) % numRandEntries) +
                        EMP[cpuNum].CP0[C0_TLBWIRED] );
   if( res != NORMAL_CODE ) {
      ReenterTC(&EMP[cpuNum]);
      /* NOT REACHED */
   }
   return res;
}

/*****************************************************************
 * REFILL_EXCEPTION 
 * 
 ****************************************************************/
static void 
REFILL_EXCEPTION(int cpuNum, int code, int isInstr, VA pc, VA addr, int isXRefill)
{
   if (isInstr) { 
      ITLB_MISS_EVENT(EmbraCpuCycleCount(cpuNum),cpuNum,addr);
   } else { 
      DTLB_MISS_EVENT(EmbraCpuCycleCount(cpuNum),cpuNum,pc,addr);
   }
   Em_EXCEPTION(cpuNum, (isXRefill? XREFILL_FLAG : 0)|REFILL_FLAG|code,0); 
}

/*****************************************************************
 * EXCEPTION - 
 * This is sort of a major routine. 
 * The CPU should abort the instruction that caused the exception
 * along with any others in the pipeline.
 * 
 * The CPU loads the Exception Program Counter (EPC) with a 
 * restart location - either the instruction, or it's predecessor
 * if it's in the branch delay slot. I check if it's in a branch
 * delay slot by seeing if the difference between PC and nPC is
 * greater than one instruction. 
 * 
 * R4000 STATUS REGISTER UPDATE 
 * 1.)  don't put in kernel mode is in kernel mode when KSU = 0 or EXL = 1
 * or ERL = 1, strictly speaking this is saved/restored by the software b/c 
 * it could either by user or supervisor modes.
 * 2.)  disable interrupts
 * 3.)  goto vector.
 * 4.)  on start cpu loads EPC with restart location...
 *  
 * 
 * checks BEV in sr.
 * but need new vector for bootstrap this is currently unused.
 *
 * This currently is set as the General Exception Handler(hardware)
 * Also Watch and FP Control Status Registers not set (would be set 
 * only when their exc occurs)
 ****************************************************************/
void Em_EXCEPTION(int cpuNum, int code, int ce)
{
   StatusReg statusReg;
   Reg causeReg;
   VA exceptionBase;
   uint prettyCode;
   int refill_flag = code & REFILL_FLAG;
   int xrefill_flag = code & XREFILL_FLAG;

   if (!EMP[cpuNum].outOfSlaveLoop)
      return;

   /*
    * To make sure that we never return to the TC from here,
    * we increment the tcGenCounter. 
    */
   if (cpuNum == curEmp->myNum) { 
      tcGenNumber++;
      exceptionDuringBackdoor = TRUE;
   }

  code &= ~(REFILL_FLAG);
  code &= ~(XREFILL_FLAG);
  prettyCode = code >> CAUSE_EXCSHIFT;

  if (DEBUG_INTR()) {
    LogEntry("EXCEPTION", cpuNum, "code: 0x%x\tintrbits: 0x%x\n",
	     code, EMP[cpuNum].intrBitsPtr[0]);
  }

  STAT_TIMER_STOP( trans_timer );

/* One more spot where we must set current cpu, because
   the timers (and perhaps other things) will cause exceptions
   on processors other than the "current" one. */
  /* Get rid of curr_cpu soon.. */
/*  curr_cpu = cpuNum; */
/*
  curEmp = &EMP[cpuNum];
*/

#ifdef DEBUG_CP0
  {
     /* Yes more than one bit can be set at once */
     char* typeOfInt = "";
     int hwIntrBits =(EMP[cpuNum].CP0[C0_CAUSE] & CAUSE_IPMASK)>>CAUSE_IPSHIFT;
     /* XXX - Hand Copied from kern/ml/SIMMP.c */
     if( hwIntrBits & 0x10 ) {
        typeOfInt = "CLOCK";
     }
     if( hwIntrBits & 0x04 ) {
        typeOfInt = "DISK or ETHER";
     }
     if( hwIntrBits & 0x20 ) {
        typeOfInt = "IPI";
     }
     if( code == EXC_CPU ) {
        CPUPrint("HW_EX %lld %d EXC %d %s CE %d PC 0x%x\n", 
                 EmbraCpuCycleCount(cpuNum),
                 cpuNum,
                 code>>2, 
                 typeOfInt,
                 ce,
                 EMP[cpuNum].PC);
     } else {
        /* Exclude UTLB misses */
        if( code == EXC_RMISS || code == EXC_WMISS ) {} else 
           CPUPrint("HW_EX %lld %d EXC %d HWBits 0x%x %-5s %-5s PC 0x%x\n", 
                    EmbraCpuCycleCount(cpuNum),
                    cpuNum, 
                    code>>2, 
                    hwIntrBits,
                    (refill_flag?"U":" "),
                    typeOfInt,
                    EMP[cpuNum].PC);
     }
  }
#endif
  STAT_INC( exceptions );
  STAT_EXC( exception_type, code>>2, refill_flag?1:0 );

   statusReg.ts_data = EMP[cpuNum].CP0[C0_SR];
   

   /*
    * Set the BD bit, CE, IP, and ExcCode fields of the Cause
    * register. Along with the BD bit, set the Exception Program
    * Counter (either to current PC or PC - 4 if BD slot).
    */
   
  causeReg = EMP[cpuNum].CP0[C0_CAUSE];
  causeReg = CAUSE_SET_EXC_NOSHIFT( causeReg, code );
  /* Read the branch delay indicator bit from the PC, */
  causeReg = CAUSE_SET_BD( causeReg, IN_BD(EMP[cpuNum].PC) );
  EMP[cpuNum].CP0[C0_CAUSE] = causeReg;

   /*Is this exception the first? (we're not in another exception) */
  if (statusReg.s32.ts_exl == 0){
    if( IN_BD( EMP[cpuNum].PC ) ) {
      /* Clear the indicator bit to make the PC usable */
      EMP[cpuNum].CP0[C0_EPC] = CLEAR_BD(EMP[cpuNum].PC) - INST_SIZE;
    } else {
      EMP[cpuNum].CP0[C0_EPC] = EMP[cpuNum].PC;
    }
    statusReg.s32.ts_exl = 1;
  }
  EMP[cpuNum].CP0[C0_SR] = statusReg.ts_data;
  

   /*The cause register is read-only.  The hardware is responsible for */
   /*clearing it */
  if( code == EXC_CPU ) {
     EMP[cpuNum].CP0[C0_CAUSE] = CAUSE_SET_CE( EMP[cpuNum].CP0[C0_CAUSE], ce );
  } else {
     EMP[cpuNum].CP0[C0_CAUSE] = CAUSE_SET_CE( EMP[cpuNum].CP0[C0_CAUSE], 0 );
  }

  VASSERT( *(unsigned*)K0_TO_MEMADDR(M_FROM_CPU(cpuNum), E_VEC),
           ("\n%d Took exception before system exception vector written\n"
	    "PC = 0x%08x\n", 
            cpuNum, EMP[cpuNum].PC) );

  
  ASSERT( statusReg.s32.ts_bev == 0);
   /* are we bootstrapping? */
  if (statusReg.s32.ts_bev == 0) { 
     exceptionBase = EXC_VEC_BASE_0;
   } else { 
      CPUError("Hello Beth and Scott. Is Emmett insane ? \n");
     exceptionBase = EXC_VEC_BASE_1;
   }

  /* I'm not even checking if it was a RESET EXCEPTION */
  /* Exception can be called from places where fp regs are saved (cp0 */
  /* and clock callouts) and from places they are not saved (cache */
  /* callouts) */
  if( refill_flag ) {
     ASSERT( embra.emode == EMBRA_PAGE || embra.sequential|| EMP[cpuNum].outTC );
     EMP[cpuNum].PC = exceptionBase;

     if (xrefill_flag) {
        EMP[cpuNum].PC = exceptionBase + XUT_VEC_OFFSET;
     }
  } else {
     ASSERT( embra.emode == EMBRA_PAGE || embra.sequential || EMP[cpuNum].outTC );
     EMP[cpuNum].PC = exceptionBase + E_VEC_OFFSET;
  }
  UpdateCPUMode(&EMP[cpuNum]);
  ASSERT (EMP[cpuNum].cpuMode == KERNEL_MODE);


  {
     /*
      *  really important:
      *  In the case of an interrupt, ipi, ... an exception
      *  can be raised on cpuNum while it is not curEmp!!!
      */
     EmbraState *backup = curEmp;
     curEmp = &EMP[cpuNum];
     if (refill_flag) {
        UTLB_EVENT();
     } else {
        EXC_EVENT(prettyCode);
     }
     curEmp = backup;
  }

  /*
   * Bill 1 cycle for the exception itself. This is really
   * important as it matches MIPSY's handling of exceptions
   */
  EMP[cpuNum].cycleCountdown--;
  
}

/* Called from emitted code to raise breakpoint and syscall (others?) */
/* exceptions */
void Em_RaiseEXCEPTION(int cpuNum, int code, int ce)
{
   Em_EXCEPTION(cpuNum, code, ce);
   ReenterTC(&EMP[cpuNum]);
   /* NOT REACHED */
}
/*R4000***********************************************************
 * ExceptionReturn
 * instruction for returning from an interrupt, exception, or
 * error trap.  if error trap load pc from the ErrorEPC and clear
 * ERL bit of the status register. else load pc from EPC and clear
 * EXL bit of the status register.
 * PC gets set in R4, not in R3.
 * Handler must save pc, op mode, status or interrupts enable, and restore.
 *****************************************************************/
void 
Em_ExceptionReturn(int cpuNum)
{
   StatusReg statusReg;
   VA restoreAddr;
   ASSERT( curEmp == &EMP[cpuNum]);
   if (!IS_KERNEL_MODE(curEmp)) {
      Em_EXCEPTION(cpuNum,EXC_CPU,0);
      ReenterTC(curEmp);
   }
   if (DEBUG_INTR()) {
     LogEntry("CPUMode", cpuNum, " ERET at %x \n",curEmp->PC);
   }

/* Annotations:*/
   ASSERT (curEmp->myNum == cpuNum);
   RFE_EVENT();

   statusReg.ts_data = EMP[cpuNum].CP0[C0_SR];

   if (statusReg.s32.ts_erl){
     restoreAddr = EMP[cpuNum].CP0[C0_ERROR_EPC];
     EMP[cpuNum].PC = restoreAddr;
     statusReg.s32.ts_erl = 0;
   } else {
     restoreAddr = EMP[cpuNum].CP0[C0_EPC];
     EMP[cpuNum].PC = restoreAddr;
     statusReg.s32.ts_exl = 0;
   }
   
   EMP[cpuNum].CP0[C0_SR] = statusReg.ts_data;
   UpdateCPUMode(&EMP[cpuNum]);

   EMP[cpuNum].LLAddr = 0;
   EMP[cpuNum].LLBit = 0;

   /*
    * Increment the cycle count, since the ERET 
    * instruction actually takes one cycle
    */
   EMP[cpuNum].cycleCountdown--;


/* check for interrupts before we leave...*/
   if (Update_And_Check_Interrupts(cpuNum,0)){
   }
   
   ReenterTC(&EMP[cpuNum]);
}

/*****************************************************************
 * RestoreFromException
 * Restore contol to a process that an exception preempted. This
 * restores the previous interrumpt mask and kernel/user mode bits
 * in the status register, moves the old to the previous, and keeps
 * the old unchanged. 
 ****************************************************************/
void 
Em_RestoreFromException( int cpuNum , VA restoreAddr )
{
 CPUError("Embra:cp0 rfe on cpu %i, curEmp->myNum=%i, ~ERT on R4000!\n",
                   cpuNum,curEmp->myNum);
    
}

/*****************************************************************
 * Em_MoveFromC0
 ****************************************************************/
uint
Em_MoveFromC0(int cpuNum, Inst instr)
{
   unsigned gp_reg =  rt(instr);  /* dest   */
   unsigned co_reg =  rd(instr);  /* source */

   /* ASSERT( gp_reg < 32 && co_reg < 32 );*/
   if (!IS_KERNEL_MODE(curEmp)) {
      Em_EXCEPTION(cpuNum,EXC_CPU,0);
      ReenterTC(&EMP[cpuNum]);
   }
   if (co_reg == C0_COUNT) { 
      /* There is one thing which could change this */
      /* somebody wrote to C0_COUNT */
      EMP[cpuNum].CP0[C0_COUNT] += (EmbraCpuCycleCount(cpuNum) 
                                    - EMP[cpuNum].timerCycleCount)/
         COUNTER_FREQUENCY_DIVIDER;
      EMP[cpuNum].CP0[C0_COUNT] &= 0xffffffff;
      EMP[cpuNum].timerCycleCount = EmbraCpuCycleCount(cpuNum);
   } else if (co_reg == C0_RAND) {
      unsigned numRandEntries = EMP[cpuNum].numTlbEntries - EMP[cpuNum].CP0[C0_TLBWIRED];
      EMP[cpuNum].CP0[C0_RAND] = EMP[cpuNum].CP0[C0_TLBWIRED] +
            ((uint32)EmbraCpuCycleCount(cpuNum)) % numRandEntries; 
   }
   if (rs(instr) == mfc_op) { 
      EMP[cpuNum].R[gp_reg] = (Reg32_s) EMP[cpuNum].CP0[co_reg]; 
   } else if (rs(instr) == dmfc_op) {
      EMP[cpuNum].R[gp_reg] = EMP[cpuNum].CP0[co_reg]; 
   } else {
      ASSERT(0);
   }
   
 
  return NORMAL_CODE;  
}

static Cp0RegControl cp0RegCtl[NUM_CP0_REGS] = CP0_REG_CONTROL_ARRAY;

/*****************************************************************
 * MoveToC0
 ****************************************************************/
uint
Em_MoveToC0(int cpuNum, Inst instr, VA nextPC )
{
   unsigned gp_reg =  rt(instr);
   unsigned co_reg =  rd(instr);
   Reg val;
   int bd = IN_BD( EMP[cpuNum].PC );
   int mode = CURRENT_MODE(&EMP[cpuNum]);

   if (!IS_KERNEL_MODE(curEmp)) {
      Em_EXCEPTION(cpuNum,EXC_CPU,0);
      ReenterTC(&EMP[cpuNum]);
   }
   /* ASSERT( gp_reg < 32 && co_reg < 32 );*/
   if (cp0RegCtl[co_reg].read_only) {
      return NORMAL_CODE;
   }
   if (cp0RegCtl[co_reg].size == -1) {
      CPUWarning("WriteC0Reg write to invalid reg %d\n",co_reg);
   }
   val =  EMP[cpuNum].R[gp_reg];
   if (rs(instr) == mtc_op) { 
      val = (Reg32_s) val;
   } else if (rs(instr) == dmtc_op) {
      /* val = val; */
   } else {
      ASSERT(0);
   }
   if ((cp0RegCtl[co_reg].zero_mask & val) != 0) {
      /* Trying to write must be zero bits, force them to zero. */
      val &= ~cp0RegCtl[co_reg].zero_mask;
      /* Print a warning message for the programmer. Special
       * case 32bit signed-extention bits that set high zero bits
       * of 64 bit registers (IRIX 6.2 does this). 
       */
   }

  switch( co_reg ) {
  case C0_TLBHI: {
     uint old_asid = GET_ASID(EMP[cpuNum].CP0[C0_TLBHI]);
     uint new_asid = GET_ASID(val);

     /* This is an MMU context switch */
     qc_mmu_switch( cpuNum, old_asid, new_asid, 0 );
     EMP[cpuNum].CP0[C0_TLBHI] = val & TLBHI_FILLMASK;
  }
     break;
  case C0_SR: {
     int modeChange = 0;
     /* Check for interrupts both before and after status register */
     /* changes  this lets us catch the intr whether we are raising or */
     /* lowering intrs */
     /* This should never succeed.  Either we delivered the interupt
        via a callback  or it was masked */
     if( Update_And_Check_Interrupts( cpuNum,0 ) ) {
        /* Take interrupt before mtc0 */
#ifdef DEBUG_EMBRA_INTR
        char* typeOfInt = "";
        int hwIntrBits =(EMP[cpuNum].CP0[C0_CAUSE] & CAUSE_IPMASK)>>CAUSE_IPSHIFT;
        /* XXX - Hand Copied from kern/ml/SIMMP.c */
        if( hwIntrBits & 0x10 ) {
           typeOfInt = "CLOCK";
        }
        if( hwIntrBits & 0x04 ) {
           typeOfInt = "DISK or ETHER";
        }
        if( hwIntrBits & 0x20 ) {
           typeOfInt = "IPI";
        }

        CPUError("EMBRA mtc0 problem  %lld %d EXC %d HWBits 0x%x %-5s PC 0x%x\n", 
                 EmbraCpuCycleCount(cpuNum),
                 cpuNum, 
                 EXC_INT>>2, 
                 hwIntrBits,
                 typeOfInt,
                 EMP[cpuNum].CP0[C0_EPC]);
#endif

        ReenterTC(&EMP[cpuNum]);
        /* NOT REACHED */
     }

     if (embraprintsr) {
        if ((((EMP[cpuNum].CP0[co_reg] & 0xff00) >> 8) != ((EMP[cpuNum].R[gp_reg] & 0xff00) >> 8)) ||
            ((EMP[cpuNum].CP0[co_reg] & 0x1) != (EMP[cpuNum].R[gp_reg] & 0x1))) {
           LogEntry("IMASK mtSR", cpuNum, "\tIM %x IEC %x -> NIM %x NIEC %x SR %x PC %x RA %x\n",
                    (EMP[cpuNum].CP0[co_reg] & 0xff00) >> 8,
                    (EMP[cpuNum].CP0[co_reg] & 0x1),
                    (EMP[cpuNum].R[gp_reg] & 0xff00) >> 8,
                    (EMP[cpuNum].R[gp_reg] & 0x1),
                    EMP[cpuNum].R[gp_reg],
                    EMP[cpuNum].PC,
                    EMP[cpuNum].R[31]);
        }
     }


     EMP[cpuNum].CP0[co_reg] = val;
     UpdateCPUMode(&EMP[cpuNum]);

     /* When the status register changes see if there are any pending */
     /* interrupts at this (possibly new) level */

     if( Update_And_Check_Interrupts( cpuNum, 
                                      (bd?nextPC:EMP[cpuNum].PC+INST_SIZE))){
        /* 
         * We took interrupt after mtc0. Skip it when we return.
         */
        
        ReenterTC(&EMP[cpuNum]);
        /* NOT REACHED */
     }
  }
  break;
     
     
  case C0_COUNT:
     EMP[cpuNum].CP0[co_reg] = val;
     EMP[cpuNum].CP0[co_reg] &= 0xffffffff;
     EMP[cpuNum].timerCycleCount = EmbraCpuCycleCount(cpuNum);
     if (EventCallbackActive(&(timerCallbackHdr[cpuNum]))) {
       EventCallbackRemove(&(timerCallbackHdr[cpuNum]));
    }
    EmbraSetTimerCallback(cpuNum);
    break;

  case C0_COMPARE:


     EMP[cpuNum].CP0[co_reg] = val;
     EMP[cpuNum].CP0[co_reg] &= 0xffffffff;

     EMP[cpuNum].CP0[C0_CAUSE] &= ~CAUSE_IP8;
     if (EventCallbackActive(&(timerCallbackHdr[cpuNum]))) {
        EventCallbackRemove(&(timerCallbackHdr[cpuNum]));
     }
     EmbraSetTimerCallback(cpuNum); 
     
     break;
  case C0_CAUSE:
     /* Only CAUSE_SW1 and CAUSE_SW2 are writable bits */
     EMP[cpuNum].CP0[C0_CAUSE] &= ~(CAUSE_SW2|CAUSE_SW1);
     EMP[cpuNum].CP0[C0_CAUSE] |= (val & (CAUSE_SW2|CAUSE_SW1)); 

     /* When the status register changes see if there are any pending */
     /* interrupts at this (possibly new) level */

     if( Update_And_Check_Interrupts( cpuNum, 
                                      (bd?nextPC:EMP[cpuNum].PC+INST_SIZE))){
        /* 
         * We took interrupt after mtc0. Skip it when we return.
         */
        
        ReenterTC(&EMP[cpuNum]);
        /* NOT REACHED */
     }

     break;

  default:
     EMP[cpuNum].CP0[co_reg] = val;
  }

  return NORMAL_CODE;  
}

/****************************************************************
 * EmbraTimerCallback
 *
 * This is the routine called back on the event that
 * the C0_COMPARE == C0_COUNT, it is  set when the 
 * COMPARE register is written.  Raise IP(7) in the 
 * C0_CAUSE register to signal a timer interrupt is 
 * pending.  Note: IP(7) is cleared when when the 
 * C0_COMPARE register is written. use DEV_IEC_MAGICERR
 * as it corresponds to hw interrupt bit #5 which raises
 * ip7 in the cause register.
 ****************************************************************/
static void 
EmbraTimerCallback(int cpuNum, EventCallbackHdr *ECBhdr, void *empty) 
{
  EMP[cpuNum].CP0[C0_CAUSE] |= CAUSE_IP8;
  if (Update_And_Check_Interrupts(cpuNum,0)){
  }
}

/*****************************************************************
 * EmbraSetTimerCallback
 *
 * Each processor has its own r4kTimerInfo and r4k_timerHdr
 * associated with it.  When the Compare register is written to
 * we check current cycle count, and, projecting into the future the
 * diff btwn the new Compare reg value and the current cycle count, a
 * callback is set to fire in that calculate amt of time.  It is tricky
 * only when they decide to write to the count reg, which is permited on
 * system initialization or to synchronize processors.  Since we do not 
 * update the Count register continually, our cycle count and the Count
 * register value could then be out-of-synch.  The r4kTimerInfo variable
 * is used to track this discrepency.
 ****************************************************************/
static void 
EmbraSetTimerCallback(int cpuNum)
{
   SimTime  timeInFuture;

   EMP[cpuNum].CP0[C0_COUNT] += 
      (EmbraCpuCycleCount(cpuNum) - EMP[cpuNum].timerCycleCount)/
      COUNTER_FREQUENCY_DIVIDER;
   EMP[cpuNum].CP0[C0_COUNT] &= 0xffffffff;
   EMP[cpuNum].timerCycleCount = EmbraCpuCycleCount(cpuNum);

   if (EMP[cpuNum].CP0[C0_COMPARE] >= EMP[cpuNum].CP0[C0_COUNT]) { 
      timeInFuture =   (EMP[cpuNum].CP0[C0_COMPARE] - EMP[cpuNum].CP0[C0_COUNT]);
      timeInFuture *=  COUNTER_FREQUENCY_DIVIDER;
   } else { 
      timeInFuture = 0x100000000LL - 
         (EMP[cpuNum].CP0[C0_COUNT] - EMP[cpuNum].CP0[C0_COMPARE]);
      timeInFuture *=  COUNTER_FREQUENCY_DIVIDER;
   }
   EventDoCallback(cpuNum, EmbraTimerCallback, 
                      &(timerCallbackHdr[cpuNum]), NULL, timeInFuture);
}

/* The kernel sets the cop1 usable flag to 0 when it  first starts a */
/* new context */ 
/* to limit the saving of FPU registers to those procs who use them. The */
/* first time a process uses a FPU instruction it  needs to trap into the */
/* kernel for it to set this bit. */
/* Called from callout in callout.s */
void 
Em_RaiseC1Unusable( int cpuNum )  
{
   ASSERT (curEmp->myNum == cpuNum);
   /* Insure that we got here properly */
   ASSERT( !(EMP[cpuNum].CP0[C0_SR] & SR_CU1 ) );
   /* I think its lousy to set the cause register before calling */
   /* EXCEPTION, but it does simplify parameter passing */
   Em_EXCEPTION(cpuNum, EXC_CPU, 1);
   ReenterTC(&EMP[cpuNum]);
   /* NOT REACHED */
}

/*****************************************************************
 * CacheOP
 ****************************************************************/
uint
Em_CacheOP(int cpuNum, Inst instr)
{
   int mode = CURRENT_MODE(&EMP[cpuNum]);

   if (!IS_KERNEL_MODE(curEmp)) {
      static int haveWarned = FALSE;
      if (!haveWarned) {
         CPUWarning("Embra: CPU %d hit cache op @ 0x%llx not in kernel mode\n",
                 cpuNum, (Reg64) curEmp->PC);
         haveWarned = TRUE;
      }
      Em_EXCEPTION(cpuNum,EXC_CPU,0);
      ReenterTC(&EMP[cpuNum]);
   }
   /* NOP for now */
   return 0;
}
/*****************************************************************
 * UpdateCPUMode
 *
 * This function is shared between mipsy and embra
 * Update with care. Do not remove the embra specific
 * stuff.
 *****************************************************************/
static void 
UpdateCPUMode(CPUState *P)
{
   StatusReg  statusReg; 
   statusReg.ts_data = P->CP0[C0_SR];
   P->cpuMode = KERNEL_MODE;
   if (!statusReg.s32.ts_erl && !statusReg.s32.ts_exl) { 
      ASSERT(statusReg.s32.ts_ksu != 3);
     if (statusReg.s32.ts_ksu == 2) {
        P->cpuMode = USER_MODE;
     } else if (statusReg.s32.ts_ksu == 1) { 
        P->cpuMode = SUPERVISOR_MODE;
     }
  }

#if defined(SIM_MIPS32)
      if ((statusReg.s32.ts_ux) || (statusReg.s32.ts_sx) ||
          (statusReg.s32.ts_kx)) {
         static int warned_64 = 0;
         if (!warned_64) {
            CPUWarning("Embra: 64-bit mode not implemented, ignoring SR write\n");
            warned_64 = 1;
         }
         
         statusReg.s32.ts_ux = 0;
         statusReg.s32.ts_sx = 0;
         statusReg.s32.ts_kx = 0;
         P->CP0[C0_SR] = statusReg.ts_data;
      }
#endif

      /* Don't allow reverse endian either. */
      if (statusReg.s32.ts_re) {
         static int warned_re = 0;
         if (!warned_re) {
            CPUWarning("Embra: reverse-endian mode not implemented, ignore SR write\n");
            warned_re = 1;
         }
         statusReg.s32.ts_re = 0;
      }

   if (DEBUG_INTR()) {
     LogEntry("CPUMode" , P->myNum, " going to %d PC=0x%llx SR=%x erl=%d exl=%d ksu=%d\n",
	      P->cpuMode, (uint64) P->PC, P->CP0[C0_SR],
	      statusReg.s32.ts_erl,statusReg.s32.ts_exl,
	      statusReg.s32.ts_ksu);
   }
   P->notFRbit = (statusReg.s32.ts_fr == 0);
   switch (P->cpuMode) { 
   case KERNEL_MODE: 
      P->mmu = P->kernelMMU;
      P->is32bitMode = (statusReg.s32.ts_kx == 0);
      break;
   case SUPERVISOR_MODE: 
      P->mmu = P->userMMU;
      P->is32bitMode = (statusReg.s32.ts_sx == 0);
#if defined(SIM_MIPS32)   /* this assert is incorrect for 64bit mode */
      ASSERT(IS_SUPERV_SEG(P->PC));
#endif
      break;
   case USER_MODE:
      P->mmu = P->userMMU;
      P->is32bitMode = (statusReg.s32.ts_ux == 0);
      ASSERT( IS_KUSEG(P->PC));
      break;
   default: 
      ASSERT(0);
   }
   

}