memory.c

思科路由器仿真器,用来仿7200系列得,可以在电脑上模拟路由器-Cisco router simulator, used to fake a 7200 series can be simulated

/* === Specific operations for MTS32 ====================================== */

/* MTS32 slow lookup */
static forced_inline 
mts32_entry_t *mts32_slow_lookup(cpu_mips_t *cpu,m_uint64_t vaddr,
                                 u_int op_code,u_int op_size,
                                 u_int op_type,m_uint64_t *data,
                                 u_int *exc)
{
   m_uint32_t hash_bucket,zone;
   mts32_entry_t *entry,new_entry;
   mts_map_t map;

   map.tlb_index = -1;
   hash_bucket = MTS32_HASH(vaddr);
   entry = cpu->mts_cache[hash_bucket];
   zone = (vaddr >> 29) & 0x7;

#if DEBUG_MTS_STATS
   cpu->mts_misses++;
#endif

   switch(zone) {
      case 0x00 ... 0x03:   /* kuseg */
         /* trigger TLB exception if no matching entry found */
         if (!cp0_tlb_lookup(cpu,vaddr,&map))
            goto err_tlb;

         if (!mts32_map(cpu,vaddr,&map,&new_entry))
            goto err_undef;
         break;

      case 0x04:   /* kseg0 */
         map.vaddr  = sign_extend(MIPS_KSEG0_BASE,32);
         map.paddr  = 0;
         map.len    = MIPS_KSEG0_SIZE;
         map.cached = TRUE;
         if (!mts32_map(cpu,vaddr,&map,&new_entry))
            goto err_undef;
         break;

      case 0x05:   /* kseg1 */
         map.vaddr  = sign_extend(MIPS_KSEG1_BASE,32);
         map.paddr  = 0;
         map.len    = MIPS_KSEG1_SIZE;
         map.cached = FALSE;
         if (!mts32_map(cpu,vaddr,&map,&new_entry))
            goto err_undef;
         break;

      case 0x06:   /* ksseg */
      case 0x07:   /* kseg3 */
         /* trigger TLB exception if no matching entry found */
         if (!cp0_tlb_lookup(cpu,vaddr,&map))
            goto err_tlb;

         if (!mts32_map(cpu,vaddr,&map,&new_entry))
            goto err_undef;
         break;
   }

   /* Get a new entry if necessary */
   if (!entry) {
      entry = mts32_alloc_entry(cpu);
      entry->pself = entry->pprev = NULL;
      entry->next = NULL;

      /* Store the entry in hash table for future use */
      cpu->mts_cache[hash_bucket] = entry;
   } else {
      /* Remove the entry from the reverse map list */
      if (entry->pprev) {
         if (entry->next)
            entry->next->pprev = entry->pprev;

         *(entry->pprev) = entry->next;
      }
   }

   /* Add this entry to the reverse map list */
   if (map.tlb_index != -1) {
      entry->pself = (mts32_entry_t **)&cpu->mts_cache[hash_bucket];
      entry->next  = cpu->mts_rmap[map.tlb_index];
      entry->pprev = (mts32_entry_t **)&cpu->mts_rmap[map.tlb_index];
      if (entry->next)
         entry->next->pprev = &entry->next;
      cpu->mts_rmap[map.tlb_index] = entry;
   }

   /* Fill the new entry or replace the previous */
   entry->phys_page = new_entry.phys_page;
   entry->start  = new_entry.start;
   entry->mask   = new_entry.mask;
   entry->action = new_entry.action;
   return entry;

 err_undef:
   mts_access_special(cpu,vaddr,MTS_ACC_U,op_code,op_type,op_size,data,exc);
   return NULL;
 err_address:
   mts_access_special(cpu,vaddr,MTS_ACC_AE,op_code,op_type,op_size,data,exc);
   return NULL;
 err_tlb:
   mts_access_special(cpu,vaddr,MTS_ACC_T,op_code,op_type,op_size,data,exc);
   return NULL;
}

/* MTS32 access */
static forced_inline void *mts32_access(cpu_mips_t *cpu,m_uint64_t vaddr,
                                        u_int op_code,u_int op_size,
                                        u_int op_type,m_uint64_t *data,
                                        u_int *exc)
{
   m_uint32_t hash_bucket;
   mts32_entry_t *entry;
   m_iptr_t haddr;
   u_int dev_id;

#if MEMLOG_ENABLE
   /* Record the memory access */
   memlog_rec_access(cpu,vaddr,*data,op_size,op_type);
#endif

   *exc = 0;
   hash_bucket = MTS32_HASH(vaddr);
   entry = cpu->mts_cache[hash_bucket];

#if DEBUG_MTS_STATS
   cpu->mts_lookups++;
#endif

   /* Slow lookup if nothing found in cache */
   if (unlikely((!entry) || unlikely((vaddr & entry->mask) != entry->start))) {
      entry = mts32_slow_lookup(cpu,vaddr,op_code,op_size,op_type,data,exc);
      if (!entry) return NULL;
   }

   /* Device access */
   if (unlikely(entry->action & MTS_DEV_MASK)) {
      dev_id = (entry->action & MTS_DEVID_MASK) >> MTS_DEVID_SHIFT;
      haddr = entry->action & MTS_DEVOFF_MASK;
      haddr += (m_uint32_t)vaddr - entry->start;

#if DEBUG_MTS_DEV
      cpu_log(cpu,"MTS32",
              "device access: vaddr=0x%llx, pc=0x%llx, dev_offset=0x%x\n",
              vaddr,cpu->pc,haddr);
#endif
      return(dev_access_fast(cpu,dev_id,haddr,op_size,op_type,data));
   }

   /* Raw memory access */
   haddr = entry->action & MTS_ADDR_MASK;
   haddr += (m_uint32_t)vaddr - entry->start;
#if MEMLOG_ENABLE
   memlog_update_read(cpu,haddr);
#endif
   return((void *)haddr);
}

/* MTS32 virtual address to physical page translation */
static fastcall int mts32_translate(cpu_mips_t *cpu,m_uint64_t vaddr,
                                    m_uint32_t *phys_page)
{   
   m_uint32_t hash_bucket,offset;
   mts32_entry_t *entry;
   m_uint64_t data = 0;
   u_int exc = 0;
   
   hash_bucket = MTS32_HASH(vaddr);
   entry = cpu->mts_cache[hash_bucket];

   /* Slow lookup if nothing found in cache */
   if (unlikely((!entry) || unlikely((vaddr & entry->mask) != entry->start))) {
      entry = mts32_slow_lookup(cpu,vaddr,MIPS_MEMOP_LOOKUP,4,MTS_READ,
                                &data,&exc);
      if (!entry)
         return(-1);
   }

   offset = vaddr - entry->start;
   *phys_page = entry->phys_page + (offset >> MIPS_MIN_PAGE_SHIFT);
   return(0);
}

/* ======================================================================== */

/* Shutdown MTS subsystem */
void mts_shutdown(cpu_mips_t *cpu)
{
   if (cpu->mts_shutdown != NULL)
      cpu->mts_shutdown(cpu);
}

/* Set the address mode */
int mts_set_addr_mode(cpu_mips_t *cpu,u_int addr_mode)
{
   if (cpu->addr_mode != addr_mode) {
      mts_shutdown(cpu);
      
      switch(addr_mode) {
         case 32:
            mts32_init(cpu);
            mts32_init_memop_vectors(cpu);
            break;
         case 64:
            mts64_init(cpu);
            mts64_init_memop_vectors(cpu);
            break;
         default:
            fprintf(stderr,
                    "mts_set_addr_mode: internal error (addr_mode=%u)\n",
                    addr_mode);
            exit(EXIT_FAILURE);
      }
   }

   return(0);
}

/* === Operations on physical memory ====================================== */

/* Copy a memory block from VM physical RAM to real host */
void physmem_copy_from_vm(vm_instance_t *vm,void *real_buffer,
                          m_uint64_t paddr,size_t len)
{
   struct vdevice *vm_ram;
   u_char *ptr;

   if ((vm_ram = dev_lookup(vm,paddr,FALSE)) != NULL) {
      assert(vm_ram->host_addr != 0);
      ptr = (u_char *)vm_ram->host_addr + (paddr - vm_ram->phys_addr);
      memcpy(real_buffer,ptr,len);
   }
}

/* Copy a memory block to VM physical RAM from real host */
void physmem_copy_to_vm(vm_instance_t *vm,void *real_buffer,
                        m_uint64_t paddr,size_t len)
{
   struct vdevice *vm_ram;
   u_char *ptr;

   if ((vm_ram = dev_lookup(vm,paddr,FALSE)) != NULL) {
      assert(vm_ram->host_addr != 0);
      ptr = (u_char *)vm_ram->host_addr + (paddr - vm_ram->phys_addr);
      memcpy(ptr,real_buffer,len);
   }
}

/* Copy a 32-bit word from the VM physical RAM to real host */
m_uint32_t physmem_copy_u32_from_vm(vm_instance_t *vm,m_uint64_t paddr)
{
   struct vdevice *dev;
   m_uint32_t offset;
   m_uint64_t tmp;
   void *ptr;

   if (unlikely((dev = dev_lookup(vm,paddr,FALSE)) == NULL))
      return(0);

   offset = paddr - dev->phys_addr;

   if ((dev->host_addr != 0) && !(dev->flags & VDEVICE_FLAG_NO_MTS_MMAP))
      ptr = (u_char *)dev->host_addr + offset;
   else {
      ptr = dev->handler(vm->boot_cpu,dev,offset,4,MTS_READ,&tmp);
      if (!ptr) return(tmp);
   }
   
   return(vmtoh32(*(m_uint32_t *)ptr));
}

/* Copy a 32-bit word to the VM physical RAM from real host */
void physmem_copy_u32_to_vm(vm_instance_t *vm,m_uint64_t paddr,m_uint32_t val)
{
   struct vdevice *dev;
   m_uint32_t offset;
   m_uint64_t tmp;
   void *ptr;

   if (unlikely((dev = dev_lookup(vm,paddr,FALSE)) == NULL))
      return;

   offset = paddr - dev->phys_addr;

   if ((dev->host_addr != 0) && !(dev->flags & VDEVICE_FLAG_NO_MTS_MMAP))
      ptr = (u_char *)dev->host_addr + offset;
   else {
      tmp = val;
      ptr = dev->handler(vm->boot_cpu,dev,offset,4,MTS_WRITE,&tmp);
      if (!ptr) return;
   }
   
   *(m_uint32_t *)ptr = htovm32(val);
}

/* Copy a 16-bit word from the VM physical RAM to real host */
m_uint16_t physmem_copy_u16_from_vm(vm_instance_t *vm,m_uint64_t paddr)
{
   struct vdevice *dev;
   m_uint32_t offset;
   m_uint64_t tmp;
   void *ptr;

   if (unlikely((dev = dev_lookup(vm,paddr,FALSE)) == NULL))
      return(0);

   offset = paddr - dev->phys_addr;

   if ((dev->host_addr != 0) && !(dev->flags & VDEVICE_FLAG_NO_MTS_MMAP))
      ptr = (u_char *)dev->host_addr + offset;
   else {
      ptr = dev->handler(vm->boot_cpu,dev,offset,2,MTS_READ,&tmp);
      if (!ptr) return(tmp);
   }
   
   return(vmtoh16(*(m_uint16_t *)ptr));
}

/* Copy a 16-bit word to the VM physical RAM from real host */
void physmem_copy_u16_to_vm(vm_instance_t *vm,m_uint64_t paddr,m_uint16_t val)
{
   struct vdevice *dev;
   m_uint32_t offset;
   m_uint64_t tmp;
   void *ptr;

   if (unlikely((dev = dev_lookup(vm,paddr,FALSE)) == NULL))
      return;

   offset = paddr - dev->phys_addr;

   if ((dev->host_addr != 0) && !(dev->flags & VDEVICE_FLAG_NO_MTS_MMAP))
      ptr = (u_char *)dev->host_addr + offset;
   else {
      tmp = val;
      ptr = dev->handler(vm->boot_cpu,dev,offset,2,MTS_WRITE,&tmp);
      if (!ptr) return;
   }
   
   *(m_uint16_t *)ptr = htovm16(val);
}

/* DMA transfer operation */
void physmem_dma_transfer(vm_instance_t *vm,m_uint64_t src,m_uint64_t dst,
                          size_t len)
{
   struct vdevice *src_dev,*dst_dev;
   u_char *sptr,*dptr;

   src_dev = dev_lookup(vm,src,FALSE);
   dst_dev = dev_lookup(vm,dst,FALSE);

   if ((src_dev != NULL) && (dst_dev != NULL)) {
      assert(src_dev->host_addr != 0);
      assert(dst_dev->host_addr != 0);
      
      sptr = (u_char *)src_dev->host_addr + (src - src_dev->phys_addr);
      dptr = (u_char *)dst_dev->host_addr + (dst - dst_dev->phys_addr);
      memcpy(dptr,sptr,len);
   } else {
      vm_log(vm,"DMA","unable to transfer from 0x%llx to 0x%llx (len=%lu)\n",
             src,dst,(u_long)len);
   }
}

/* strlen in VM physical memory */
size_t physmem_strlen(vm_instance_t *vm,m_uint64_t paddr)
{
   struct vdevice *vm_ram;
   size_t len = 0;
   char *ptr;
   
   if ((vm_ram = dev_lookup(vm,paddr,TRUE)) != NULL) {
      ptr = (char *)vm_ram->host_addr + (paddr - vm_ram->phys_addr);
      len = strlen(ptr);
   }

   return(len);
}

/* Physical memory dump (32-bit words) */
void physmem_dump_vm(vm_instance_t *vm,m_uint64_t paddr,m_uint32_t u32_count)
{
   m_uint32_t i;

   for(i=0;i<u32_count;i++) {
      vm_log(vm,"physmem_dump","0x%8.8llx: 0x%8.8x\n",
             paddr+(i<<2),physmem_copy_u32_from_vm(vm,paddr+(i<<2)));
   }
}