dev_gt.c

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

      /* Fill bits 9:14 */
      res |= (tmp & 0x3f) << 9;
   }

   return(res);
}

/*
 * Walk through the Ethernet hash table.
 */
static int gt_eth_hash_lookup(struct gt_data *d,struct eth_port *port,
                              n_eth_addr_t *addr,m_uint64_t *entry)
{
   m_uint64_t eth_val;
   m_uint32_t hte_addr;
   u_int hash_val;
   int i;

   eth_val  = (m_uint64_t)addr->eth_addr_byte[0] << 3;
   eth_val |= (m_uint64_t)addr->eth_addr_byte[1] << 11;
   eth_val |= (m_uint64_t)addr->eth_addr_byte[2] << 19;
   eth_val |= (m_uint64_t)addr->eth_addr_byte[3] << 27;
   eth_val |= (m_uint64_t)addr->eth_addr_byte[4] << 35;
   eth_val |= (m_uint64_t)addr->eth_addr_byte[5] << 43;

   /* Compute hash value for Ethernet address filtering */
   hash_val = gt_eth_hash_value(addr,port->pcr & GT_PCR_HM);
   
   if (port->pcr & GT_PCR_HS) {
      /* 1/2K address filtering */
      hte_addr = port->ht_addr + ((hash_val & 0x7ff) << 3);
   } else {
      /* 8K address filtering */
      hte_addr = port->ht_addr + (hash_val << 3);
   }

#if DEBUG_ETH_HASH
   GT_LOG(d,"Hash Lookup for Ethernet address "
          "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x: addr=0x%x\n",
          addr->eth_addr_byte[0], addr->eth_addr_byte[1],
          addr->eth_addr_byte[2], addr->eth_addr_byte[3],
          addr->eth_addr_byte[4], addr->eth_addr_byte[5],
          hte_addr);
#endif

   for(i=0;i<GT_HTE_HOPNUM;i++,hte_addr+=8) {
      *entry  = ((m_uint64_t)physmem_copy_u32_from_vm(d->vm,hte_addr)) << 32;
      *entry |= physmem_copy_u32_from_vm(d->vm,hte_addr+4);

      /* Empty entry ? */
      if (!(*entry & GT_HTE_VALID))
         return(GT_HTLOOKUP_MISS);

      /* Skip flag or different Ethernet address: jump to next entry */
      if ((*entry & GT_HTE_SKIP) || ((*entry & GT_HTE_ADDR_MASK) != eth_val))
         continue;

      /* We have the good MAC address in this entry */
      return(GT_HTLOOKUP_MATCH);
   }

   return(GT_HTLOOKUP_HOP_EXCEEDED);
}

/* 
 * Check if a packet (given its destination address) must be handled 
 * at RX path.
 *
 * Return values:
 *   - 0: Discard packet ;
 *   - 1: Receive packet ;
 *   - 2: Receive packet and set "M" bit in RX descriptor.
 *
 * The documentation is not clear about the M bit in RX descriptor.
 * It is described as "Miss" or "Match" depending on the section.
 */
static inline int gt_eth_handle_rx_daddr(struct gt_data *d,
                                         struct eth_port *port,
                                         u_int hash_res,
                                         m_uint64_t hash_entry)
{
   /* Hop Number exceeded */
   if (hash_res == GT_HTLOOKUP_HOP_EXCEEDED)
      return(1);

   /* Match and hash entry marked as "Receive" */
   if ((hash_res == GT_HTLOOKUP_MATCH) && (hash_entry & GT_HTE_RD))
      return(2);

   /* Miss but hash table default mode to forward ? */
   if ((hash_res == GT_HTLOOKUP_MISS) && (port->pcr & GT_PCR_HDM))
      return(2);

   /* Promiscous Mode */
   if (port->pcr & GT_PCR_PM)
      return(1);

   /* Drop packet for other cases */
   return(0);
}

/* Put a packet in buffer of a descriptor */
static void gt_eth_rxdesc_put_pkt(struct gt_data *d,struct eth_desc *rxd,
                                  u_char **pkt,ssize_t *pkt_len)
{
   ssize_t len,cp_len;

   len = (rxd->buf_size & GT_RXDESC_BS_MASK) >> GT_RXDESC_BS_SHIFT;
   
   /* compute the data length to copy */
   cp_len = m_min(len,*pkt_len);
   
   /* copy packet data to the VM physical RAM */
   physmem_copy_to_vm(d->vm,*pkt,rxd->buf_ptr,cp_len);
      
   /* set the byte count in descriptor */
   rxd->buf_size |= cp_len;

   *pkt += cp_len;
   *pkt_len -= cp_len;
}

/* Put a packet in the specified RX queue */
static int gt_eth_handle_rxqueue(struct gt_data *d,u_int port_id,u_int queue,
                                 u_char *pkt,ssize_t pkt_len)
{
   struct eth_port *port = &d->eth_ports[port_id];
   m_uint32_t rx_start,rx_current;
   struct eth_desc rxd0,rxdn,*rxdc;
   ssize_t tot_len = pkt_len;
   u_char *pkt_ptr = pkt;
   n_eth_dot1q_hdr_t *hdr;
   m_uint64_t hash_entry;
   int i,hash_res,addr_action;

   /* Truncate the packet if it is too big */
   pkt_len = m_min(pkt_len,GT_MAX_PKT_SIZE);

   /* Copy the first RX descriptor */
   if (!(rx_start = rx_current = port->rx_start[queue]))
      goto dma_error;

   /* Analyze the Ethernet header */
   hdr = (n_eth_dot1q_hdr_t *)pkt;

   /* Hash table lookup for address filtering */
   hash_res = gt_eth_hash_lookup(d,port,&hdr->daddr,&hash_entry);

#if DEBUG_ETH_HASH
   GT_LOG(d,"Hash result: %d, hash_entry=0x%llx\n",hash_res,hash_entry);
#endif

   if (!(addr_action = gt_eth_handle_rx_daddr(d,port,hash_res,hash_entry)))
      return(FALSE);

   /* Load the first RX descriptor */
   gt_eth_desc_read(d,rx_start,&rxd0);

#if DEBUG_ETH_RX
   GT_LOG(d,"port %u/queue %u: reading desc at 0x%8.8x "
          "[buf_size=0x%8.8x,cmd_stat=0x%8.8x,"
          "next_ptr=0x%8.8x,buf_ptr=0x%8.8x]\n",
          port_id,queue,rx_start,
          rxd0.buf_size,rxd0.cmd_stat,rxd0.next_ptr,rxd0.buf_ptr);
#endif

   for(i=0,rxdc=&rxd0;tot_len>0;i++)
   {
      /* We must own the descriptor */
      if (!(rxdc->cmd_stat & GT_RXDESC_OWN))
         goto dma_error;

      /* Put data into the descriptor buffer */
      gt_eth_rxdesc_put_pkt(d,rxdc,&pkt_ptr,&tot_len);

      /* Clear the OWN bit */
      rxdc->cmd_stat &= ~GT_RXDESC_OWN;

      /* We have finished if the complete packet has been stored */
      if (tot_len == 0) {
         rxdc->cmd_stat |= GT_RXDESC_L;
         rxdc->buf_size += 4;  /* Add 4 bytes for CRC */
      }

      /* Update the descriptor in host memory (but not the 1st) */
      if (i != 0)
         gt_eth_desc_write(d,rx_current,rxdc);

      /* Get address of the next descriptor */
      rx_current = rxdc->next_ptr;

      if (tot_len == 0)
         break;

      if (!rx_current)
         goto dma_error;

      /* Read the next descriptor from VM physical RAM */
      gt_eth_desc_read(d,rx_current,&rxdn);
      rxdc = &rxdn;
   }

   /* Update the RX pointers */
   port->rx_start[queue] = port->rx_current[queue] = rx_current;

   /* Update the first RX descriptor */
   rxd0.cmd_stat |= GT_RXDESC_F;

   if (hash_res == GT_HTLOOKUP_HOP_EXCEEDED)
      rxd0.cmd_stat |= GT_RXDESC_HE;

   if (addr_action == 2)
      rxd0.cmd_stat |= GT_RXDESC_M;

   if (ntohs(hdr->type) <= N_ETH_MTU)   /* 802.3 frame */
      rxd0.cmd_stat |= GT_RXDESC_FT;

   gt_eth_desc_write(d,rx_start,&rxd0);

   /* Update MIB counters */
   port->rx_bytes += pkt_len;
   port->rx_frames++;

   /* Indicate that we have a frame ready */
   port->icr |= (GT_ICR_RXBUFQ0 << queue) | GT_ICR_RXBUF;
   gt_eth_update_int_status(d,port); 
   return(TRUE);

 dma_error:
   port->icr |= (GT_ICR_RXERRQ0 << queue) | GT_ICR_RXERR;
   gt_eth_update_int_status(d,port); 
   return(FALSE);
}

/* Handle RX packet for an Ethernet port */
static int gt_eth_handle_rx_pkt(netio_desc_t *nio,
                                u_char *pkt,ssize_t pkt_len,
                                struct gt_data *d,void *arg)
{
   u_int queue,port_id = (int)arg;
   struct eth_port *port;

   port = &d->eth_ports[port_id];

  /* Check if RX DMA is active */
   if (!(port->sdcmr & GT_SDCMR_ERD))
      return(FALSE);

   queue = 0;  /* At this time, only put packet in queue 0 */
   gt_eth_handle_rxqueue(d,port_id,queue,pkt,pkt_len);
   return(TRUE);
}

/* Shutdown a GT system controller */
void dev_gt_shutdown(vm_instance_t *vm,struct gt_data *d)
{
   if (d != NULL) {
      /* Stop the TX ring scanner */
      ptask_remove(d->eth_tx_tid);

      /* Remove the device */
      dev_remove(vm,&d->dev);

      /* Remove the PCI device */
      pci_dev_remove(d->pci_dev);

      /* Free the structure itself */
      free(d);
   }
}

/* Create a new GT64010 controller */
int dev_gt64010_init(vm_instance_t *vm,char *name,
                     m_uint64_t paddr,m_uint32_t len,u_int irq)
{
   struct gt_data *d;

   if (!(d = malloc(sizeof(*d)))) {
      fprintf(stderr,"gt64010: unable to create device data.\n");
      return(-1);
   }

   memset(d,0,sizeof(*d));
   d->vm = vm;   
   d->bus[0] = vm->pci_bus[0];

   vm_object_init(&d->vm_obj);
   d->vm_obj.name = name;
   d->vm_obj.data = d;
   d->vm_obj.shutdown = (vm_shutdown_t)dev_gt_shutdown;

   dev_init(&d->dev);
   d->dev.name      = name;
   d->dev.priv_data = d;
   d->dev.phys_addr = paddr;
   d->dev.phys_len  = len;
   d->dev.handler   = dev_gt64010_access;

   /* Add the controller as a PCI device */
   if (!pci_dev_lookup(d->bus[0],0,0,0)) {
      d->pci_dev = pci_dev_add(d->bus[0],name,
                               PCI_VENDOR_GALILEO,PCI_PRODUCT_GALILEO_GT64010,
                               0,0,irq,d,NULL,NULL,NULL);

      if (!d->pci_dev) {
         fprintf(stderr,"gt64010: unable to create PCI device.\n");
         return(-1);
      }
   }

   /* Map this device to the VM */
   vm_bind_device(vm,&d->dev);
   vm_object_add(vm,&d->vm_obj);
   return(0);
}

/*
 * pci_gt64120_read()
 *
 * Read a PCI register.
 */
static m_uint32_t pci_gt64120_read(cpu_mips_t *cpu,struct pci_device *dev,
                                   int reg)
{   
   switch (reg) {
      case 0x08:
         return(0x03008005);
      default:
         return(0);
   }
}

/* Create a new GT64120 controller */
int dev_gt64120_init(vm_instance_t *vm,char *name,
                     m_uint64_t paddr,m_uint32_t len,u_int irq)
{
   struct gt_data *d;

   if (!(d = malloc(sizeof(*d)))) {
      fprintf(stderr,"gt64120: unable to create device data.\n");
      return(-1);
   }

   memset(d,0,sizeof(*d));
   d->vm = vm;
   d->bus[0] = vm->pci_bus[0];
   d->bus[1] = vm->pci_bus[1];

   vm_object_init(&d->vm_obj);
   d->vm_obj.name = name;
   d->vm_obj.data = d;
   d->vm_obj.shutdown = (vm_shutdown_t)dev_gt_shutdown;

   dev_init(&d->dev);
   d->dev.name      = name;
   d->dev.priv_data = d;
   d->dev.phys_addr = paddr;
   d->dev.phys_len  = len;
   d->dev.handler   = dev_gt64120_access;

   /* Add the controller as a PCI device */
   if (!pci_dev_lookup(d->bus[0],0,0,0)) {
      d->pci_dev = pci_dev_add(d->bus[0],name,
                               PCI_VENDOR_GALILEO,PCI_PRODUCT_GALILEO_GT64120,
                               0,0,irq,d,NULL,pci_gt64120_read,NULL);
      if (!d->pci_dev) {
         fprintf(stderr,"gt64120: unable to create PCI device.\n");
         return(-1);
      }
   }

   /* Map this device to the VM */
   vm_bind_device(vm,&d->dev);
   vm_object_add(vm,&d->vm_obj);
   return(0);
}

/*
 * pci_gt96100_read()
 *
 * Read a PCI register.
 */
static m_uint32_t pci_gt96100_read(cpu_mips_t *cpu,struct pci_device *dev,
                                   int reg)
{   
   switch (reg) {
      case 0x08:
         return(0x03008005);
      default:
         return(0);
   }
}

/* Create a new GT96100 controller */
int dev_gt96100_init(vm_instance_t *vm,char *name,
                     m_uint64_t paddr,m_uint32_t len,
                     u_int dma_irq,u_int eth_irq)
{
   struct gt_data *d;

   if (!(d = malloc(sizeof(*d)))) {
      fprintf(stderr,"gt96100: unable to create device data.\n");
      return(-1);
   }

   memset(d,0,sizeof(*d));
   d->name = name;
   d->vm = vm;
   d->eth_irq = eth_irq;
   d->bus[0] = vm->pci_bus[0];
   d->bus[1] = vm->pci_bus[1];

   vm_object_init(&d->vm_obj);
   d->vm_obj.name = name;
   d->vm_obj.data = d;
   d->vm_obj.shutdown = (vm_shutdown_t)dev_gt_shutdown;

   dev_init(&d->dev);
   d->dev.name      = name;
   d->dev.priv_data = d;
   d->dev.phys_addr = paddr;
   d->dev.phys_len  = len;
   d->dev.handler   = dev_gt96100_access;

   /* Add the controller as a PCI device */
   if (!pci_dev_lookup(d->bus[0],0,0,0)) {
      d->pci_dev = pci_dev_add(d->bus[0],name,
                               PCI_VENDOR_GALILEO,PCI_PRODUCT_GALILEO_GT96100,
                               0,0,dma_irq,d,NULL,pci_gt96100_read,NULL);
      if (!d->pci_dev) {
         fprintf(stderr,"gt96100: unable to create PCI device.\n");
         return(-1);
      }
   }

   /* Start the TX ring scanner */
   d->eth_tx_tid = ptask_add((ptask_callback)gt_eth_handle_txqueues,d,NULL);

   /* Map this device to the VM */
   vm_bind_device(vm,&d->dev);
   vm_object_add(vm,&d->vm_obj);
   return(0);
}

/* Bind a NIO to GT96100 device */
int dev_gt96100_set_nio(struct gt_data *d,u_int port_id,netio_desc_t *nio)
{
   struct eth_port *port;

   if (port_id >= GT_ETH_PORTS)
      return(-1);

   port = &d->eth_ports[port_id];

   /* check that a NIO is not already bound */
   if (port->nio != NULL)
      return(-1);

   port->nio = nio;
   netio_rxl_add(nio,(netio_rx_handler_t)gt_eth_handle_rx_pkt,
                 d,(void *)port_id);
   return(0);
}

/* Unbind a NIO from a GT96100 device */
int dev_gt96100_unset_nio(struct gt_data *d,u_int port_id)
{
   struct eth_port *port;

   if (port_id >= GT_ETH_PORTS)
      return(-1);

   port = &d->eth_ports[port_id];

   if (port->nio != NULL) {
      netio_rxl_remove(port->nio);
      port->nio = NULL;
   }

   return(0);
}