eth_switch.c

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

/*
 * Cisco 7200 (Predator) simulation platform.
 * Copyright (c) 2006 Christophe Fillot ([e-mail])
 *
 * Virtual Ethernet switch with VLAN/Trunk support.
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <errno.h>
#include <sys/select.h>
#include <sys/time.h>
#include <sys/types.h>
#include <assert.h>

#include "utils.h"
#include "net.h"
#include "registry.h"
#include "net_io.h"
#include "eth_switch.h"

/* Debbuging message */
void ethsw_debug(ethsw_table_t *t,char *fmt,...)
{
   char module[128];
   va_list ap;

   if (t->debug) {
      va_start(ap,fmt);
      snprintf(module,sizeof(module),"ETHSW %s",t->name);
      m_flog(log_file,module,fmt,ap);
      va_end(ap);
   }
}


/* Compute hash index on the specified MAC address and VLAN */
static inline u_int ethsw_hash_index(n_eth_addr_t *addr,u_int vlan_id)
{
   u_int h_index;

   h_index =  (addr->eth_addr_byte[0] << 8) | addr->eth_addr_byte[1];
   h_index ^= (addr->eth_addr_byte[2] << 8) | addr->eth_addr_byte[3];
   h_index ^= (addr->eth_addr_byte[4] << 8) | addr->eth_addr_byte[5];
   h_index ^= vlan_id;
   return(h_index & (ETHSW_HASH_SIZE - 1));
}

/* Invalidate the whole MAC address table */
static void ethsw_invalidate(ethsw_table_t *t)
{
   memset(t->mac_addr_table,0,sizeof(t->mac_addr_table));
}

/* Invalidate entry of the MAC address table referring to the specified NIO */
static void ethsw_invalidate_port(ethsw_table_t *t,netio_desc_t *nio)
{
   ethsw_mac_entry_t *entry;
   int i;
   
   for(i=0;i<ETHSW_HASH_SIZE;i++) {
      entry = &t->mac_addr_table[i];
      if (entry->nio == nio) {
         entry->nio = NULL;
         entry->vlan_id = 0;
      }
   }
}

/* Push a 802.1Q tag */
static void dot1q_push_tag(m_uint8_t *pkt,ethsw_packet_t *sp,u_int vlan)
{   
   n_eth_dot1q_hdr_t *hdr;

   memcpy(pkt,sp->pkt,(N_ETH_HLEN - 2));

   hdr = (n_eth_dot1q_hdr_t *)pkt;
   hdr->type    = htons(N_ETH_PROTO_DOT1Q);
   hdr->vlan_id = htons(sp->input_vlan);

   memcpy(pkt + sizeof(n_eth_dot1q_hdr_t),
          sp->pkt + (N_ETH_HLEN - 2),
          sp->pkt_len - (N_ETH_HLEN - 2));
}

/* Pop a 802.1Q tag */
static void dot1q_pop_tag(m_uint8_t *pkt,ethsw_packet_t *sp)
{
   memcpy(pkt,sp->pkt,(N_ETH_HLEN - 2));

   memcpy(pkt + (N_ETH_HLEN - 2),
          sp->pkt + sizeof(n_eth_dot1q_hdr_t),
          sp->pkt_len - sizeof(n_eth_dot1q_hdr_t));
}

/* Input vector for ACCESS ports */
static void ethsw_iv_access(ethsw_table_t *t,ethsw_packet_t *sp,
                            netio_desc_t *op)
{
   m_uint8_t pkt[ETHSW_MAX_PKT_SIZE+4];

   switch(op->vlan_port_type) {
      /* Access -> Access: no special treatment */
      case ETHSW_PORT_TYPE_ACCESS:  
         netio_send(op,sp->pkt,sp->pkt_len);
         break;

      /* Access -> 802.1Q: push tag */
      case ETHSW_PORT_TYPE_DOT1Q:
         /* 
          * If the native VLAN of output port is the same as input,
          * forward the packet without adding the tag.
          */
         if (op->vlan_id == sp->input_vlan) {
            netio_send(op,sp->pkt,sp->pkt_len);
         } else {
            dot1q_push_tag(pkt,sp,op->vlan_id);
            netio_send(op,pkt,sp->pkt_len+4);
         }
         break;

      default:
         fprintf(stderr,"ethsw_iv_access: unknown port type %u\n",
                 op->vlan_port_type);
   }
}

/* Input vector for 802.1Q ports */
static void ethsw_iv_dot1q(ethsw_table_t *t,ethsw_packet_t *sp,
                           netio_desc_t *op)
{
   m_uint8_t pkt[ETHSW_MAX_PKT_SIZE+4];

   /* If we don't have an input tag, we work temporarily as an access port */
   if (!sp->input_tag) {
      ethsw_iv_access(t,sp,op);
      return;
   }

   switch(op->vlan_port_type) {
      /* 802.1Q -> Access: pop tag */
      case ETHSW_PORT_TYPE_ACCESS:
         dot1q_pop_tag(pkt,sp);
         netio_send(op,pkt,sp->pkt_len-4);
         break;

      /* 802.1Q -> 802.1Q: pop tag if native VLAN in output otherwise no-op */
      case ETHSW_PORT_TYPE_DOT1Q:
         if (op->vlan_id == sp->input_vlan) {
            dot1q_pop_tag(pkt,sp);
            netio_send(op,pkt,sp->pkt_len-4);
         } else {
            netio_send(op,sp->pkt,sp->pkt_len);
         }
         break;

      default:
         fprintf(stderr,"ethsw_iv_dot1q: unknown port type %u\n",
                 op->vlan_port_type);
   }
}

/* Flood a packet */
static void ethsw_flood(ethsw_table_t *t,ethsw_packet_t *sp)
{
   ethsw_input_vector_t input_vector;
   netio_desc_t *op;
   int i;

   input_vector = sp->input_port->vlan_input_vector;
   assert(input_vector != NULL);

   for(i=0;i<ETHSW_MAX_NIO;i++) {
      op = t->nio[i];

      if (!op || (op == sp->input_port))
         continue;

      /* skip output port configured in access mode with a different vlan */
      if ((op->vlan_port_type == ETHSW_PORT_TYPE_ACCESS) &&
          (op->vlan_id != sp->input_vlan))
         continue;

      /* send the packet on output port */
      input_vector(t,sp,op);
   }
}

/* Forward a packet */
static void ethsw_forward(ethsw_table_t *t,ethsw_packet_t *sp)
{
   n_eth_hdr_t *hdr = (n_eth_hdr_t *)sp->pkt;
   ethsw_input_vector_t input_vector;
   ethsw_mac_entry_t *entry;
   u_int h_index;

   /* Learn the source MAC address */
   h_index = ethsw_hash_index(&hdr->saddr,sp->input_vlan);
   entry = &t->mac_addr_table[h_index];

   entry->nio      = sp->input_port;
   entry->vlan_id  = sp->input_vlan;
   entry->mac_addr = hdr->saddr;

   /* If we have a broadcast/multicast packet, flood it */
   if (eth_addr_is_mcast(&hdr->daddr)) {
      ethsw_debug(t,"multicast dest, flooding packet.\n");
      ethsw_flood(t,sp);
      return;
   }

   /* Lookup on the destination MAC address (unicast) */
   h_index = ethsw_hash_index(&hdr->daddr,sp->input_vlan);
   entry = &t->mac_addr_table[h_index];

   /* If the dest MAC is unknown, flood the packet */
   if (memcmp(&entry->mac_addr,&hdr->daddr,N_ETH_ALEN) || 
       (entry->vlan_id != sp->input_vlan)) 
   {
      ethsw_debug(t,"unknown dest, flooding packet.\n");
      ethsw_flood(t,sp);
      return;
   }

   /* Forward the packet to the output port only */
   if (entry->nio != sp->input_port) {
      input_vector = sp->input_port->vlan_input_vector;
      assert(input_vector != NULL);
      input_vector(t,sp,entry->nio);
   } else {
      ethsw_debug(t,"source and dest ports identical, dropping.\n");
   }
}

/* Receive a packet and prepare its forwarding */
static inline int ethsw_receive(ethsw_table_t *t,netio_desc_t *nio,
                                u_char *pkt,ssize_t pkt_len)
{   
   n_eth_dot1q_hdr_t *dot1q_hdr;
   n_eth_isl_hdr_t *isl_hdr;
   n_eth_hdr_t *eth_hdr;
   n_eth_llc_hdr_t *llc_hdr;
   ethsw_packet_t sp;
   u_char *ptr;

   sp.input_port = nio;
   sp.input_vlan = 0;
   sp.pkt        = pkt;
   sp.pkt_len    = pkt_len;

   /* Skip runt packets */
   if (sp.pkt_len < N_ETH_HLEN)
      return(-1);

   /* Determine the input VLAN */
   switch(nio->vlan_port_type) {
      case ETHSW_PORT_TYPE_ACCESS:
         sp.input_vlan = nio->vlan_id;
         break;

      case ETHSW_PORT_TYPE_DOT1Q:
         dot1q_hdr = (n_eth_dot1q_hdr_t *)sp.pkt;

         /* use the native VLAN if no tag is found */
         if (ntohs(dot1q_hdr->type) != N_ETH_PROTO_DOT1Q) {
            sp.input_vlan = nio->vlan_id;
            sp.input_tag  = FALSE;
         } else {
            sp.input_vlan = ntohs(dot1q_hdr->vlan_id) & 0xFFF;
            sp.input_tag  = TRUE;
         }
         break;

      case ETHSW_PORT_TYPE_ISL:
         /* Check that we have an ISL packet */
         eth_hdr = (n_eth_hdr_t *)pkt;

         if (!eth_addr_is_cisco_isl(&eth_hdr->daddr))
            break;

         /* Verify LLC header */
         llc_hdr = PTR_ADJUST(n_eth_llc_hdr_t *,eth_hdr,sizeof(n_eth_hdr_t));
         if (!eth_llc_check_snap(llc_hdr))
            break;

         /* Get the VLAN id */
         isl_hdr = PTR_ADJUST(n_eth_isl_hdr_t *,llc_hdr,
                              sizeof(n_eth_llc_hdr_t));
         ptr = (u_char *)&isl_hdr->vlan;
         sp.input_vlan = (((u_int)ptr[0] << 8) | ptr[1]) >> 1;
         break;

      default:
         fprintf(stderr,"ethsw_receive: unknown port type %u\n",
                 nio->vlan_port_type);
         return(-1);
   }

   if (sp.input_vlan != 0)
      ethsw_forward(t,&sp);
   return(0);
}

/* Receive a packet (handle the locking part) */
static int ethsw_recv_pkt(netio_desc_t *nio,u_char *pkt,ssize_t pkt_len,
                          ethsw_table_t *t)
{
   ETHSW_LOCK(t);
   ethsw_receive(t,nio,pkt,pkt_len);
   ETHSW_UNLOCK(t);
   return(0);
}

/* Set a port as an access port with the specified VLAN */
static void set_access_port(netio_desc_t *nio,u_int vlan_id)
{
   nio->vlan_port_type    = ETHSW_PORT_TYPE_ACCESS;
   nio->vlan_id           = vlan_id;
   nio->vlan_input_vector = ethsw_iv_access;
}

/* Set a port as a 802.1Q trunk port */
static void set_dot1q_port(netio_desc_t *nio,u_int native_vlan)
{
   nio->vlan_port_type    = ETHSW_PORT_TYPE_DOT1Q;
   nio->vlan_id           = native_vlan;
   nio->vlan_input_vector = ethsw_iv_dot1q;
}

/* Acquire a reference to an Ethernet switch (increment reference count) */
ethsw_table_t *ethsw_acquire(char *name)
{
   return(registry_find(name,OBJ_TYPE_ETHSW));
}

/* Release an Ethernet switch (decrement reference count) */
int ethsw_release(char *name)
{
   return(registry_unref(name,OBJ_TYPE_ETHSW));
}

/* Create a virtual ethernet switch */
ethsw_table_t *ethsw_create(char *name)
{
   ethsw_table_t *t;

   /* Allocate a new switch structure */
   if (!(t = malloc(sizeof(*t))))
      return NULL;

   memset(t,0,sizeof(*t));
   pthread_mutex_init(&t->lock,NULL);

   if (!(t->name = strdup(name)))
      goto err_name;

   /* Record this object in registry */
   if (registry_add(t->name,OBJ_TYPE_ETHSW,t) == -1) {
      fprintf(stderr,"ethsw_create: unable to register switch '%s'\n",name);
      goto err_reg;
   }

   return t;

 err_reg:
   free(t->name);
 err_name:
   free(t);
   return NULL;
}

/* Add a NetIO descriptor to a virtual ethernet switch */
int ethsw_add_netio(ethsw_table_t *t,char *nio_name)
{
   netio_desc_t *nio;
   int i;

   ETHSW_LOCK(t);

   /* Try to find a free slot in the NIO array */
   for(i=0;i<ETHSW_MAX_NIO;i++)
      if (t->nio[i] == NULL)
         break;
   
   /* No free slot found ... */
   if (i == ETHSW_MAX_NIO)
      goto error;

   /* Acquire the NIO descriptor and increment its reference count */
   if (!(nio = netio_acquire(nio_name)))
      goto error;

   /* By default, the port is an access port in VLAN 1 */
   set_access_port(nio,1);