frame_relay.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])
 *
 * Frame-Relay switch.
 */

#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 "utils.h"
#include "mempool.h"
#include "registry.h"
#include "net_io.h"
#include "frame_relay.h"

#define DEBUG_FRSW  0

/* Number of LMI trailing bytes */
#define LMI_TRAILING_SIZE  3

extern FILE *log_file;

/* ANSI LMI packet header */   
static const m_uint8_t lmi_ansi_hdr[] = { 
   0x00, 0x01, 0x03, 0x08, 0x00, 0x75, 0x95,
};

/* DLCI hash function */
static inline u_int frsw_dlci_hash(u_int dlci)
{
   return((dlci ^ (dlci >> 8)) & (FRSW_HASH_SIZE-1));
}

/* DLCI lookup */
frsw_conn_t *frsw_dlci_lookup(frsw_table_t *t,netio_desc_t *input,u_int dlci)
{
   frsw_conn_t *vc;
   
   for(vc=t->dlci_table[frsw_dlci_hash(dlci)];vc;vc=vc->hash_next)
      if ((vc->input == input) && (vc->dlci_in == dlci))
         return vc;

   return NULL;
}

/* Handle a ANSI LMI packet */
ssize_t frsw_handle_lmi_ansi_pkt(frsw_table_t *t,netio_desc_t *input,
                                 m_uint8_t *pkt,ssize_t len)
{
   m_uint8_t resp[FR_MAX_PKT_SIZE],*pres,*preq;
   m_uint8_t itype,isize;
   int msg_type,seq_ok;
   ssize_t rlen;
   frsw_conn_t *sc;
   u_int dlci;

   if ((len <= (sizeof(lmi_ansi_hdr) + LMI_TRAILING_SIZE)) || 
       memcmp(pkt,lmi_ansi_hdr,sizeof(lmi_ansi_hdr)))
      return(-1);

   len -= LMI_TRAILING_SIZE;

#if DEBUG_FRSW
   m_log(input->name,"received an ANSI LMI packet:\n");
   mem_dump(log_file,pkt,len);
#endif

   /* Prepare response packet */
   memcpy(resp,lmi_ansi_hdr,sizeof(lmi_ansi_hdr));
   resp[FR_LMI_ANSI_STATUS_OFFSET] = FR_LMI_ANSI_STATUS;

   preq = &pkt[sizeof(lmi_ansi_hdr)];
   pres = &resp[sizeof(lmi_ansi_hdr)];
   
   msg_type = -1;
   seq_ok = FALSE;

   while((preq + 2) < (pkt + len)) {
      /* get item type and size */
      itype = preq[0];
      isize = preq[1];

      /* check packet boundary */
      if ((preq + isize + 2) > (pkt + len)) {
         m_log(input->name,"invalid LMI packet:\n");
         mem_dump(log_file,pkt,len);
         return(-1);
      }

      switch(itype) {
         case 0x01:   /* report information element */
            if (isize != 1) {
               m_log(input->name,"invalid LMI item size.\n");
               return(-1);
            }

            if ((msg_type = preq[2]) > 1) {
               m_log(input->name,"unknown LMI report type 0x%x.\n",msg_type);
               return(-1);
            }

            pres[0] = 0x01;
            pres[1] = 0x01;
            pres[2] = msg_type;
            pres += 3;
            break;

         case 0x03:   /* sequences */
            if (isize != 2) {
               m_log(input->name,"invalid LMI item size.\n");
               return(-1);
            }

            pres[0] = 0x03;
            pres[1] = 0x02;

            if (input->fr_lmi_seq != preq[3]) {
               m_log(input->name,"resynchronization with LMI sequence...\n");
               input->fr_lmi_seq = preq[3];
            } 

            input->fr_lmi_seq++;
            if (!input->fr_lmi_seq) input->fr_lmi_seq++;
            pres[2] = input->fr_lmi_seq;
            pres[3] = preq[2];

#if DEBUG_FRSW
            m_log(input->name,"iSSN=0x%x, iRSN=0x%x, oSSN=0x%x, oRSN=0x%x\n",
                  preq[2],preq[3],pres[2],pres[3]);
#endif
            pres += 4;
            seq_ok = TRUE;
            break;
            
         default:
            m_log(input->name,"unknown LMI item type %u\n",itype);
            goto done;
      }

      /* proceed next item */
      preq += isize + 2;
   }

 done:
   if ((msg_type == -1) || !seq_ok) {
       m_log(input->name,"incomplete LMI packet.\n");
       return(-1);
   }

   /* full status, send DLCI info */
   if (msg_type == 0) {
#if DEBUG_FRSW
      m_log(input->name,"LMI full status, advertising DLCIs\n");
#endif
      for(sc=input->fr_conn_list;sc;sc=sc->next) {
         dlci = sc->dlci_in;
#if DEBUG_FRSW
         m_log(input->name,"sending LMI adv for DLCI %u\n",dlci);
#endif
         pres[0] = 0x07;
         pres[1] = 0x03;
         pres[2] = dlci >> 4;
         pres[3] = 0x80 | ((dlci & 0x0f) << 3);
         pres[4] = 0x82;
         pres += 5;
      }
   }

   /* it seems that a trailing is required */
   memset(pres,0,LMI_TRAILING_SIZE);
   pres += LMI_TRAILING_SIZE;
   rlen = pres - resp;

#if DEBUG_FRSW
   m_log(input->name,"sending ANSI LMI packet:\n");
   mem_dump(log_file,resp,rlen);
#endif

   netio_send(input,resp,rlen);
   return(0);
}

/* DLCI switching */
void frsw_dlci_switch(frsw_conn_t *vc,m_uint8_t *pkt)
{
   pkt[0] = (pkt[0] & 0x03) | ((vc->dlci_out >> 4) << 2);
   pkt[1] = (pkt[1] & 0x0f) | ((vc->dlci_out & 0x0f) << 4);

   /* update the statistics counter */
   vc->count++;
}

/* Handle a Frame-Relay packet */
ssize_t frsw_handle_pkt(frsw_table_t *t,netio_desc_t *input,
                        m_uint8_t *pkt,ssize_t len)
{
   netio_desc_t *output = NULL;
   frsw_conn_t *vc;
   m_uint32_t dlci;
   ssize_t slen;

   /* Extract DLCI information */
   dlci =  ((pkt[0] & 0xfc) >> 2) << 4;
   dlci |= (pkt[1] & 0xf0) >> 4;

#if DEBUG_FRSW
   m_log(input->name,"Trying to switch packet with input DLCI %u.\n",dlci);
   mem_dump(log_file,pkt,len);
#endif

   /* LMI ? */
   if (dlci == FR_DLCI_LMI_ANSI)
      return(frsw_handle_lmi_ansi_pkt(t,input,pkt,len));

   /* DLCI switching */
   if ((vc = frsw_dlci_lookup(t,input,dlci)) != NULL) {
      frsw_dlci_switch(vc,pkt);
      output = vc->output;
   } 

#if DEBUG_FRSW
   if (output) {
      m_log(input->name,"Switching packet to interface %s.\n",output->name);
   } else {
      m_log(input->name,"Unable to switch packet.\n");
   }
#endif

   /* Send the packet on output interface */
   slen = netio_send(output,pkt,len);

   if (len != slen) {
      t->drop++;
      return(-1);
   }

   return(0);
}

/* Receive a Frame-Relay packet */
static int frsw_recv_pkt(netio_desc_t *nio,u_char *pkt,ssize_t pkt_len,
                         frsw_table_t *t)
{
   int res;

   FRSW_LOCK(t);
   res = frsw_handle_pkt(t,nio,pkt,pkt_len);
   FRSW_UNLOCK(t);
   return(res);
}

/* Acquire a reference to a Frame-Relay switch (increment reference count) */
frsw_table_t *frsw_acquire(char *name)
{
   return(registry_find(name,OBJ_TYPE_FRSW));
}

/* Release a Frame-Relay switch (decrement reference count) */
int frsw_release(char *name)
{
   return(registry_unref(name,OBJ_TYPE_FRSW));
}

/* Create a virtual switch table */
frsw_table_t *frsw_create_table(char *name)
{
   frsw_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);
   mp_create_fixed_pool(&t->mp,"Frame-Relay Switch");

   if (!(t->name = mp_strdup(&t->mp,name)))
      goto err_name;

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

   return t;

 err_reg:
 err_name:
   mp_free_pool(&t->mp);
   free(t);
   return NULL;
}

/* Unlink a VC */
static void frsw_unlink_vc(frsw_conn_t *vc)
{
   if (vc) {
      if (vc->next)
         vc->next->pprev = vc->pprev;

      if (vc->pprev)
         *(vc->pprev) = vc->next;
   }
}

/* Free resources used by a VC */
static void frsw_release_vc(frsw_conn_t *vc)
{
   if (vc) {
      /* release input NIO */
      if (vc->input) {
         netio_rxl_remove(vc->input);