dynamips/trunk/frame_relay.c

/*
 * Cisco router simulation platform.
 * Copyright (c) 2006 Christophe Fillot (cf@utc.fr)
 *
 * 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

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)) || 
       memcmp(pkt,lmi_ansi_hdr,sizeof(lmi_ansi_hdr)))
      return(-1);

#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;
      }
   }

   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);
         netio_release(vc->input->name);
      }

      /* release output NIO */
      if (vc->output) 
         netio_release(vc->output->name);
   }
}

/* Free resources used by a Frame-Relay switch */
static int frsw_free(void *data,void *arg)
{
   frsw_table_t *t = data;
   frsw_conn_t *vc;
   int i;

   for(i=0;i<FRSW_HASH_SIZE;i++)
      for(vc=t->dlci_table[i];vc;vc=vc->hash_next)
         frsw_release_vc(vc);

   mp_free_pool(&t->mp);
   free(t);
   return(TRUE);
}

/* Delete a Frame-Relay switch */
int frsw_delete(char *name)
{
   return(registry_delete_if_unused(name,OBJ_TYPE_FRSW,frsw_free,NULL));
}

/* Delete all Frame-Relay switches */
int frsw_delete_all(void)
{
   return(registry_delete_type(OBJ_TYPE_FRSW,frsw_free,NULL));
}

/* Create a switch connection */
int frsw_create_vc(frsw_table_t *t,char *nio_input,u_int dlci_in,
                   char *nio_output,u_int dlci_out)
{
   frsw_conn_t *vc,**p;
   u_int hbucket;

   FRSW_LOCK(t);

   /* Allocate a new VC */
   if (!(vc = mp_alloc(&t->mp,sizeof(*vc)))) {
      FRSW_UNLOCK(t);
      return(-1);
   }   

   vc->input    = netio_acquire(nio_input);
   vc->output   = netio_acquire(nio_output);
   vc->dlci_in  = dlci_in;
   vc->dlci_out = dlci_out;
   
   /* Check these NIOs are valid and the input VC does not exists */
   if (!vc->input || !vc->output)
      goto error;

   if (frsw_dlci_lookup(t,vc->input,dlci_in)) {
      fprintf(stderr,"FRSW %s: switching for VC %u on IF %s "
              "already defined.\n",t->name,dlci_in,vc->input->name);
      goto error;
   }

   /* Add as a RX listener */
   if (netio_rxl_add(vc->input,(netio_rx_handler_t)frsw_recv_pkt,t,NULL) == -1)
      goto error;

   hbucket = frsw_dlci_hash(dlci_in);
   vc->hash_next = t->dlci_table[hbucket];
   t->dlci_table[hbucket] = vc;

   for(p=(frsw_conn_t **)&vc->input->fr_conn_list;*p;p=&(*p)->next)
      if ((*p)->dlci_in > dlci_in)
         break;

   vc->next = *p;
   if (*p) (*p)->pprev = &vc->next;
   vc->pprev = p;
   *p = vc;

   FRSW_UNLOCK(t);
   return(0);

 error:
   FRSW_UNLOCK(t);
   frsw_release_vc(vc);
   mp_free(vc);
   return(-1);
}

/* Remove a switch connection */
int frsw_delete_vc(frsw_table_t *t,char *nio_input,u_int dlci_in,
                   char *nio_output,u_int dlci_out)
{
   netio_desc_t *input,*output;
   frsw_conn_t **vc,*p;
   u_int hbucket;

   FRSW_LOCK(t);

   input = registry_exists(nio_input,OBJ_TYPE_NIO);
   output = registry_exists(nio_output,OBJ_TYPE_NIO);

   if (!input || !output) {
      FRSW_UNLOCK(t);
      return(-1);
   }

   hbucket = frsw_dlci_hash(dlci_in);
   for(vc=&t->dlci_table[hbucket];*vc;vc=&(*vc)->hash_next) 
   {
      p = *vc;

      if ((p->input == input) && (p->output == output) &&
          (p->dlci_in == dlci_in) && (p->dlci_out == dlci_out))
      {
         /* Found a matching VC, remove it */
         *vc = (*vc)->hash_next;
         frsw_unlink_vc(p);
         FRSW_UNLOCK(t);

         /* Release NIOs */
         frsw_release_vc(p);
         mp_free(p);
         return(0);
      }
   }

   FRSW_UNLOCK(t);
   return(-1);
}

/* Save the configuration of a Frame-Relay switch */
void frsw_save_config(frsw_table_t *t,FILE *fd)
{
   frsw_conn_t *vc;
   int i;

   fprintf(fd,"frsw create %s\n",t->name);

   FRSW_LOCK(t);

   for(i=0;i<FRSW_HASH_SIZE;i++) {
      for(vc=t->dlci_table[i];vc;vc=vc->next) {
         fprintf(fd,"frsw create_vc %s %s %u %s %u\n",
                 t->name,vc->input->name,vc->dlci_in,
                 vc->output->name,vc->dlci_out);
      }
   }

   FRSW_UNLOCK(t);

   fprintf(fd,"\n");
}

/* Save configurations of all Frame-Relay switches */
static void frsw_reg_save_config(registry_entry_t *entry,void *opt,int *err)
{
   frsw_save_config((frsw_table_t *)entry->data,(FILE *)opt);
}

void frsw_save_config_all(FILE *fd)
{
   registry_foreach_type(OBJ_TYPE_FRSW,frsw_reg_save_config,fd,NULL);
}

/* Create a new interface */
int frsw_cfg_create_if(frsw_table_t *t,char **tokens,int count)
{
   netio_desc_t *nio = NULL;
   int nio_type;

   /* at least: IF, interface name, NetIO type */
   if (count < 3) {
      fprintf(stderr,"frsw_cfg_create_if: invalid interface description\n");
      return(-1);
   }

   nio_type = netio_get_type(tokens[2]);
   switch(nio_type) {
      case NETIO_TYPE_UNIX:
         if (count != 5) {
            fprintf(stderr,"FRSW: invalid number of arguments "
                    "for UNIX NIO '%s'\n",tokens[1]);
            break;
         }

         nio = netio_desc_create_unix(tokens[1],tokens[3],tokens[4]);
         break;

      case NETIO_TYPE_UDP:
         if (count != 6) {
            fprintf(stderr,"FRSW: invalid number of arguments "
                    "for UDP NIO '%s'\n",tokens[1]);
            break;
         }

         nio = netio_desc_create_udp(tokens[1],atoi(tokens[3]),
                                     tokens[4],atoi(tokens[5]));
         break;

      case NETIO_TYPE_TCP_CLI:
         if (count != 5) {
            fprintf(stderr,"FRSW: invalid number of arguments "
                    "for TCP CLI NIO '%s'\n",tokens[1]);
            break;
         }

         nio = netio_desc_create_tcp_cli(tokens[1],tokens[3],tokens[4]);
         break;

      case NETIO_TYPE_TCP_SER:
         if (count != 4) {
            fprintf(stderr,"FRSW: invalid number of arguments "
                    "for TCP SER NIO '%s'\n",tokens[1]);
            break;
         }

         nio = netio_desc_create_tcp_ser(tokens[1],tokens[3]);
         break;

      default:
         fprintf(stderr,"FRSW: unknown/invalid NETIO type '%s'\n",
                 tokens[2]);
   }

   if (!nio) {
      fprintf(stderr,"FRSW: unable to create NETIO descriptor of "
              "interface %s\n",tokens[1]);
      return(-1);
   }

   netio_release(nio->name);
   return(0);
}

/* Create a new virtual circuit */
int frsw_cfg_create_vc(frsw_table_t *t,char **tokens,int count)
{
   /* 5 parameters: "VC", InputIF, InDLCI, OutputIF, OutDLCI */
   if (count != 5) {
      fprintf(stderr,"FRSW: invalid VPC descriptor.\n");
      return(-1);
   }

   return(frsw_create_vc(t,tokens[1],atoi(tokens[2]),
                         tokens[3],atoi(tokens[4])));
}

#define FRSW_MAX_TOKENS  16

/* Handle a FRSW configuration line */
int frsw_handle_cfg_line(frsw_table_t *t,char *str)
{  
   char *tokens[FRSW_MAX_TOKENS];
   int count;

   if ((count = m_strsplit(str,':',tokens,FRSW_MAX_TOKENS)) <= 1)
      return(-1);

   if (!strcmp(tokens[0],"IF"))
      return(frsw_cfg_create_if(t,tokens,count));
   else if (!strcmp(tokens[0],"VC"))
      return(frsw_cfg_create_vc(t,tokens,count));

   fprintf(stderr,"FRSW: Unknown statement \"%s\" (allowed: IF,VC)\n",
           tokens[0]);
   return(-1);
}

/* Read a FRSW configuration file */
int frsw_read_cfg_file(frsw_table_t *t,char *filename)
{
   char buffer[1024],*ptr;
   FILE *fd;

   if (!(fd = fopen(filename,"r"))) {
      perror("fopen");
      return(-1);
   }
   
   while(!feof(fd)) {
      if (!fgets(buffer,sizeof(buffer),fd))
         break;
      
      /* skip comments and end of line */
      if ((ptr = strpbrk(buffer,"#\r\n")) != NULL)
         *ptr = 0;

      /* analyze non-empty lines */
      if (strchr(buffer,':'))
         frsw_handle_cfg_line(t,buffer);
   }
   
   fclose(fd);
   return(0);
}

/* Start a virtual Frame-Relay switch */
int frsw_start(char *filename)
{
   frsw_table_t *t;

   if (!(t = frsw_create_table("default"))) {
      fprintf(stderr,"FRSW: unable to create virtual fabric table.\n");
      return(-1);
   }

   if (frsw_read_cfg_file(t,filename) == -1) {
      fprintf(stderr,"FRSW: unable to parse configuration file.\n");
      return(-1);
   }
   
   frsw_release("default");
   return(0);
}
1	dpavlin	1	/*
2	dpavlin	7	* Cisco router simulation platform.
3	dpavlin	1	* Copyright (c) 2006 Christophe Fillot (cf@utc.fr)
4			*
5			* Frame-Relay switch.
6			*/
7
8			#include <stdio.h>
9			#include <stdlib.h>
10			#include <string.h>
11			#include <unistd.h>
12			#include <pthread.h>
13			#include <errno.h>
14			#include <sys/select.h>
15			#include <sys/time.h>
16			#include <sys/types.h>
17
18			#include "utils.h"
19			#include "mempool.h"
20			#include "registry.h"
21			#include "net_io.h"
22			#include "frame_relay.h"
23
24			#define DEBUG_FRSW 0
25
26			extern FILE *log_file;
27
28			/* ANSI LMI packet header */
29			static const m_uint8_t lmi_ansi_hdr[] = {
30			0x00, 0x01, 0x03, 0x08, 0x00, 0x75, 0x95,
31			};
32
33			/* DLCI hash function */
34			static inline u_int frsw_dlci_hash(u_int dlci)
35			{
36			return((dlci ^ (dlci >> 8)) & (FRSW_HASH_SIZE-1));
37			}
38
39			/* DLCI lookup */
40			frsw_conn_t frsw_dlci_lookup(frsw_table_t t,netio_desc_t *input,u_int dlci)
41			{
42			frsw_conn_t *vc;
43
44			for(vc=t->dlci_table[frsw_dlci_hash(dlci)];vc;vc=vc->hash_next)
45			if ((vc->input == input) && (vc->dlci_in == dlci))
46			return vc;
47
48			return NULL;
49			}
50
51			/* Handle a ANSI LMI packet */
52			ssize_t frsw_handle_lmi_ansi_pkt(frsw_table_t t,netio_desc_t input,
53			m_uint8_t *pkt,ssize_t len)
54			{
55			m_uint8_t resp[FR_MAX_PKT_SIZE],pres,preq;
56			m_uint8_t itype,isize;
57			int msg_type,seq_ok;
58			ssize_t rlen;
59			frsw_conn_t *sc;
60			u_int dlci;
61
62	dpavlin	11	if ((len <= sizeof(lmi_ansi_hdr)) \|\|
63	dpavlin	1	memcmp(pkt,lmi_ansi_hdr,sizeof(lmi_ansi_hdr)))
64			return(-1);
65
66			#if DEBUG_FRSW
67			m_log(input->name,"received an ANSI LMI packet:\n");
68			mem_dump(log_file,pkt,len);
69			#endif
70
71			/* Prepare response packet */
72			memcpy(resp,lmi_ansi_hdr,sizeof(lmi_ansi_hdr));
73			resp[FR_LMI_ANSI_STATUS_OFFSET] = FR_LMI_ANSI_STATUS;
74
75			preq = &pkt[sizeof(lmi_ansi_hdr)];
76			pres = &resp[sizeof(lmi_ansi_hdr)];
77
78			msg_type = -1;
79			seq_ok = FALSE;
80
81			while((preq + 2) < (pkt + len)) {
82			/* get item type and size */
83			itype = preq[0];
84			isize = preq[1];
85
86			/* check packet boundary */
87			if ((preq + isize + 2) > (pkt + len)) {
88			m_log(input->name,"invalid LMI packet:\n");
89			mem_dump(log_file,pkt,len);
90			return(-1);
91			}
92
93			switch(itype) {
94			case 0x01: /* report information element */
95			if (isize != 1) {
96			m_log(input->name,"invalid LMI item size.\n");
97			return(-1);
98			}
99
100			if ((msg_type = preq[2]) > 1) {
101			m_log(input->name,"unknown LMI report type 0x%x.\n",msg_type);
102			return(-1);
103			}
104
105			pres[0] = 0x01;
106			pres[1] = 0x01;
107			pres[2] = msg_type;
108			pres += 3;
109			break;
110
111			case 0x03: /* sequences */
112			if (isize != 2) {
113			m_log(input->name,"invalid LMI item size.\n");
114			return(-1);
115			}
116
117			pres[0] = 0x03;
118			pres[1] = 0x02;
119
120			if (input->fr_lmi_seq != preq[3]) {
121			m_log(input->name,"resynchronization with LMI sequence...\n");
122			input->fr_lmi_seq = preq[3];
123			}
124
125			input->fr_lmi_seq++;
126			if (!input->fr_lmi_seq) input->fr_lmi_seq++;
127			pres[2] = input->fr_lmi_seq;
128			pres[3] = preq[2];
129
130			#if DEBUG_FRSW
131			m_log(input->name,"iSSN=0x%x, iRSN=0x%x, oSSN=0x%x, oRSN=0x%x\n",
132			preq[2],preq[3],pres[2],pres[3]);
133			#endif
134			pres += 4;
135			seq_ok = TRUE;
136			break;
137
138			default:
139			m_log(input->name,"unknown LMI item type %u\n",itype);
140			goto done;
141			}
142
143			/* proceed next item */
144			preq += isize + 2;
145			}
146
147			done:
148			if ((msg_type == -1) \|\| !seq_ok) {
149			m_log(input->name,"incomplete LMI packet.\n");
150			return(-1);
151			}
152
153			/* full status, send DLCI info */
154			if (msg_type == 0) {
155			#if DEBUG_FRSW
156			m_log(input->name,"LMI full status, advertising DLCIs\n");
157			#endif
158			for(sc=input->fr_conn_list;sc;sc=sc->next) {
159			dlci = sc->dlci_in;
160			#if DEBUG_FRSW
161			m_log(input->name,"sending LMI adv for DLCI %u\n",dlci);
162			#endif
163			pres[0] = 0x07;
164			pres[1] = 0x03;
165			pres[2] = dlci >> 4;
166			pres[3] = 0x80 \| ((dlci & 0x0f) << 3);
167			pres[4] = 0x82;
168			pres += 5;
169			}
170			}
171
172			rlen = pres - resp;
173
174			#if DEBUG_FRSW
175			m_log(input->name,"sending ANSI LMI packet:\n");
176			mem_dump(log_file,resp,rlen);
177			#endif
178
179			netio_send(input,resp,rlen);
180			return(0);
181			}
182
183			/* DLCI switching */
184			void frsw_dlci_switch(frsw_conn_t vc,m_uint8_t pkt)
185			{
186			pkt[0] = (pkt[0] & 0x03) \| ((vc->dlci_out >> 4) << 2);
187			pkt[1] = (pkt[1] & 0x0f) \| ((vc->dlci_out & 0x0f) << 4);
188
189			/* update the statistics counter */
190			vc->count++;
191			}
192
193			/* Handle a Frame-Relay packet */
194			ssize_t frsw_handle_pkt(frsw_table_t t,netio_desc_t input,
195			m_uint8_t *pkt,ssize_t len)
196			{
197			netio_desc_t *output = NULL;
198			frsw_conn_t *vc;
199			m_uint32_t dlci;
200			ssize_t slen;
201
202			/* Extract DLCI information */
203			dlci = ((pkt[0] & 0xfc) >> 2) << 4;
204			dlci \|= (pkt[1] & 0xf0) >> 4;
205
206			#if DEBUG_FRSW
207			m_log(input->name,"Trying to switch packet with input DLCI %u.\n",dlci);
208			mem_dump(log_file,pkt,len);
209			#endif
210
211			/* LMI ? */
212			if (dlci == FR_DLCI_LMI_ANSI)
213			return(frsw_handle_lmi_ansi_pkt(t,input,pkt,len));
214
215			/* DLCI switching */
216			if ((vc = frsw_dlci_lookup(t,input,dlci)) != NULL) {
217			frsw_dlci_switch(vc,pkt);
218			output = vc->output;
219			}
220
221			#if DEBUG_FRSW
222			if (output) {
223			m_log(input->name,"Switching packet to interface %s.\n",output->name);
224			} else {
225			m_log(input->name,"Unable to switch packet.\n");
226			}
227			#endif
228
229			/* Send the packet on output interface */
230			slen = netio_send(output,pkt,len);
231
232			if (len != slen) {
233			t->drop++;
234			return(-1);
235			}
236
237			return(0);
238			}
239
240			/* Receive a Frame-Relay packet */
241			static int frsw_recv_pkt(netio_desc_t nio,u_char pkt,ssize_t pkt_len,
242			frsw_table_t *t)
243			{
244			int res;
245
246			FRSW_LOCK(t);
247			res = frsw_handle_pkt(t,nio,pkt,pkt_len);
248			FRSW_UNLOCK(t);
249			return(res);
250			}
251
252			/* Acquire a reference to a Frame-Relay switch (increment reference count) */
253			frsw_table_t frsw_acquire(char name)
254			{
255			return(registry_find(name,OBJ_TYPE_FRSW));
256			}
257
258			/* Release a Frame-Relay switch (decrement reference count) */
259			int frsw_release(char *name)
260			{
261			return(registry_unref(name,OBJ_TYPE_FRSW));
262			}
263
264			/* Create a virtual switch table */
265			frsw_table_t frsw_create_table(char name)
266			{
267			frsw_table_t *t;
268
269			/* Allocate a new switch structure */
270			if (!(t = malloc(sizeof(*t))))
271			return NULL;
272
273			memset(t,0,sizeof(*t));
274			pthread_mutex_init(&t->lock,NULL);
275			mp_create_fixed_pool(&t->mp,"Frame-Relay Switch");
276
277			if (!(t->name = mp_strdup(&t->mp,name)))
278			goto err_name;
279
280			/* Record this object in registry */
281			if (registry_add(t->name,OBJ_TYPE_FRSW,t) == -1) {
282			fprintf(stderr,"frsw_create_table: unable to create switch '%s'\n",name);
283			goto err_reg;
284			}
285
286			return t;
287
288			err_reg:
289			err_name:
290			mp_free_pool(&t->mp);
291			free(t);
292			return NULL;
293			}
294
295			/* Unlink a VC */
296			static void frsw_unlink_vc(frsw_conn_t *vc)
297			{
298			if (vc) {
299			if (vc->next)
300			vc->next->pprev = vc->pprev;
301
302			if (vc->pprev)
303			*(vc->pprev) = vc->next;
304			}
305			}
306
307			/* Free resources used by a VC */
308			static void frsw_release_vc(frsw_conn_t *vc)
309			{
310			if (vc) {
311			/* release input NIO */
312			if (vc->input) {
313			netio_rxl_remove(vc->input);
314			netio_release(vc->input->name);
315			}
316
317			/* release output NIO */
318			if (vc->output)
319			netio_release(vc->output->name);
320			}
321			}
322
323			/* Free resources used by a Frame-Relay switch */
324			static int frsw_free(void data,void arg)
325			{
326			frsw_table_t *t = data;
327			frsw_conn_t *vc;
328			int i;
329
330			for(i=0;i<FRSW_HASH_SIZE;i++)
331			for(vc=t->dlci_table[i];vc;vc=vc->hash_next)
332			frsw_release_vc(vc);
333
334			mp_free_pool(&t->mp);
335			free(t);
336			return(TRUE);
337			}
338
339			/* Delete a Frame-Relay switch */
340			int frsw_delete(char *name)
341			{
342			return(registry_delete_if_unused(name,OBJ_TYPE_FRSW,frsw_free,NULL));
343			}
344
345			/* Delete all Frame-Relay switches */
346			int frsw_delete_all(void)
347			{
348			return(registry_delete_type(OBJ_TYPE_FRSW,frsw_free,NULL));
349			}
350
351			/* Create a switch connection */
352			int frsw_create_vc(frsw_table_t t,char nio_input,u_int dlci_in,
353			char *nio_output,u_int dlci_out)
354			{
355			frsw_conn_t vc,*p;
356			u_int hbucket;
357
358			FRSW_LOCK(t);
359
360			/* Allocate a new VC */
361			if (!(vc = mp_alloc(&t->mp,sizeof(*vc)))) {
362			FRSW_UNLOCK(t);
363			return(-1);
364			}
365
366			vc->input = netio_acquire(nio_input);
367			vc->output = netio_acquire(nio_output);
368			vc->dlci_in = dlci_in;
369			vc->dlci_out = dlci_out;
370
371			/* Check these NIOs are valid and the input VC does not exists */
372			if (!vc->input \|\| !vc->output)
373			goto error;
374
375			if (frsw_dlci_lookup(t,vc->input,dlci_in)) {
376			fprintf(stderr,"FRSW %s: switching for VC %u on IF %s "
377			"already defined.\n",t->name,dlci_in,vc->input->name);
378			goto error;
379			}
380
381			/* Add as a RX listener */
382			if (netio_rxl_add(vc->input,(netio_rx_handler_t)frsw_recv_pkt,t,NULL) == -1)
383			goto error;
384
385			hbucket = frsw_dlci_hash(dlci_in);
386			vc->hash_next = t->dlci_table[hbucket];
387			t->dlci_table[hbucket] = vc;
388
389			for(p=(frsw_conn_t *)&vc->input->fr_conn_list;p;p=&(*p)->next)
390			if ((*p)->dlci_in > dlci_in)
391			break;
392
393			vc->next = *p;
394			if (p) (p)->pprev = &vc->next;
395			vc->pprev = p;
396			*p = vc;
397
398			FRSW_UNLOCK(t);
399			return(0);
400
401			error:
402			FRSW_UNLOCK(t);
403			frsw_release_vc(vc);
404			mp_free(vc);
405			return(-1);
406			}
407
408			/* Remove a switch connection */
409			int frsw_delete_vc(frsw_table_t t,char nio_input,u_int dlci_in,
410			char *nio_output,u_int dlci_out)
411			{
412			netio_desc_t input,output;
413			frsw_conn_t *vc,p;
414			u_int hbucket;
415
416			FRSW_LOCK(t);
417
418			input = registry_exists(nio_input,OBJ_TYPE_NIO);
419			output = registry_exists(nio_output,OBJ_TYPE_NIO);
420
421			if (!input \|\| !output) {
422			FRSW_UNLOCK(t);
423			return(-1);
424			}
425
426			hbucket = frsw_dlci_hash(dlci_in);
427			for(vc=&t->dlci_table[hbucket];vc;vc=&(vc)->hash_next)
428			{
429			p = *vc;
430
431			if ((p->input == input) && (p->output == output) &&
432			(p->dlci_in == dlci_in) && (p->dlci_out == dlci_out))
433			{
434			/* Found a matching VC, remove it */
435			vc = (vc)->hash_next;
436			frsw_unlink_vc(p);
437			FRSW_UNLOCK(t);
438
439			/* Release NIOs */
440			frsw_release_vc(p);
441	dpavlin	11	mp_free(p);
442	dpavlin	1	return(0);
443			}
444			}
445
446			FRSW_UNLOCK(t);
447			return(-1);
448			}
449
450			/* Save the configuration of a Frame-Relay switch */
451			void frsw_save_config(frsw_table_t t,FILE fd)
452			{
453			frsw_conn_t *vc;
454			int i;
455
456			fprintf(fd,"frsw create %s\n",t->name);
457
458			FRSW_LOCK(t);
459
460			for(i=0;i<FRSW_HASH_SIZE;i++) {
461			for(vc=t->dlci_table[i];vc;vc=vc->next) {
462			fprintf(fd,"frsw create_vc %s %s %u %s %u\n",
463			t->name,vc->input->name,vc->dlci_in,
464			vc->output->name,vc->dlci_out);
465			}
466			}
467
468			FRSW_UNLOCK(t);
469
470			fprintf(fd,"\n");
471			}
472
473			/* Save configurations of all Frame-Relay switches */
474			static void frsw_reg_save_config(registry_entry_t entry,void opt,int *err)
475			{
476			frsw_save_config((frsw_table_t )entry->data,(FILE )opt);
477			}
478
479			void frsw_save_config_all(FILE *fd)
480			{
481			registry_foreach_type(OBJ_TYPE_FRSW,frsw_reg_save_config,fd,NULL);
482			}
483
484			/* Create a new interface */
485			int frsw_cfg_create_if(frsw_table_t t,char *tokens,int count)
486			{
487			netio_desc_t *nio = NULL;
488			int nio_type;
489
490			/* at least: IF, interface name, NetIO type */
491			if (count < 3) {
492			fprintf(stderr,"frsw_cfg_create_if: invalid interface description\n");
493			return(-1);
494			}
495
496			nio_type = netio_get_type(tokens[2]);
497			switch(nio_type) {
498			case NETIO_TYPE_UNIX:
499			if (count != 5) {
500			fprintf(stderr,"FRSW: invalid number of arguments "
501			"for UNIX NIO '%s'\n",tokens[1]);
502			break;
503			}
504
505			nio = netio_desc_create_unix(tokens[1],tokens[3],tokens[4]);
506			break;
507
508			case NETIO_TYPE_UDP:
509			if (count != 6) {
510			fprintf(stderr,"FRSW: invalid number of arguments "
511			"for UDP NIO '%s'\n",tokens[1]);
512			break;
513			}
514
515			nio = netio_desc_create_udp(tokens[1],atoi(tokens[3]),
516			tokens[4],atoi(tokens[5]));
517			break;
518
519			case NETIO_TYPE_TCP_CLI:
520			if (count != 5) {
521			fprintf(stderr,"FRSW: invalid number of arguments "
522			"for TCP CLI NIO '%s'\n",tokens[1]);
523			break;
524			}
525
526			nio = netio_desc_create_tcp_cli(tokens[1],tokens[3],tokens[4]);
527			break;
528
529			case NETIO_TYPE_TCP_SER:
530			if (count != 4) {
531			fprintf(stderr,"FRSW: invalid number of arguments "
532			"for TCP SER NIO '%s'\n",tokens[1]);
533			break;
534			}
535
536			nio = netio_desc_create_tcp_ser(tokens[1],tokens[3]);
537			break;
538
539			default:
540			fprintf(stderr,"FRSW: unknown/invalid NETIO type '%s'\n",
541			tokens[2]);
542			}
543
544			if (!nio) {
545			fprintf(stderr,"FRSW: unable to create NETIO descriptor of "
546			"interface %s\n",tokens[1]);
547			return(-1);
548			}
549
550			netio_release(nio->name);
551			return(0);
552			}
553
554			/* Create a new virtual circuit */
555			int frsw_cfg_create_vc(frsw_table_t t,char *tokens,int count)
556			{
557			/* 5 parameters: "VC", InputIF, InDLCI, OutputIF, OutDLCI */
558			if (count != 5) {
559			fprintf(stderr,"FRSW: invalid VPC descriptor.\n");
560			return(-1);
561			}
562
563			return(frsw_create_vc(t,tokens[1],atoi(tokens[2]),
564			tokens[3],atoi(tokens[4])));
565			}
566
567			#define FRSW_MAX_TOKENS 16
568
569			/* Handle a FRSW configuration line */
570			int frsw_handle_cfg_line(frsw_table_t t,char str)
571			{
572			char *tokens[FRSW_MAX_TOKENS];
573			int count;
574
575			if ((count = m_strsplit(str,':',tokens,FRSW_MAX_TOKENS)) <= 1)
576			return(-1);
577
578			if (!strcmp(tokens[0],"IF"))
579			return(frsw_cfg_create_if(t,tokens,count));
580			else if (!strcmp(tokens[0],"VC"))
581			return(frsw_cfg_create_vc(t,tokens,count));
582
583			fprintf(stderr,"FRSW: Unknown statement \"%s\" (allowed: IF,VC)\n",
584			tokens[0]);
585			return(-1);
586			}
587
588			/* Read a FRSW configuration file */
589			int frsw_read_cfg_file(frsw_table_t t,char filename)
590			{
591			char buffer[1024],*ptr;
592			FILE *fd;
593
594			if (!(fd = fopen(filename,"r"))) {
595			perror("fopen");
596			return(-1);
597			}
598
599			while(!feof(fd)) {
600			if (!fgets(buffer,sizeof(buffer),fd))
601			break;
602
603			/* skip comments and end of line */
604			if ((ptr = strpbrk(buffer,"#\r\n")) != NULL)
605			*ptr = 0;
606
607			/* analyze non-empty lines */
608			if (strchr(buffer,':'))
609			frsw_handle_cfg_line(t,buffer);
610			}
611
612			fclose(fd);
613			return(0);
614			}
615
616			/* Start a virtual Frame-Relay switch */
617			int frsw_start(char *filename)
618			{
619			frsw_table_t *t;
620
621			if (!(t = frsw_create_table("default"))) {
622			fprintf(stderr,"FRSW: unable to create virtual fabric table.\n");
623			return(-1);
624			}
625
626			if (frsw_read_cfg_file(t,filename) == -1) {
627			fprintf(stderr,"FRSW: unable to parse configuration file.\n");
628			return(-1);
629			}
630
631			frsw_release("default");
632			return(0);
633			}