/[dynamips]/upstream/dynamips-0.2.7-RC1/frame_relay.c
This is repository of my old source code which isn't updated any more. Go to git.rot13.org for current projects!
ViewVC logotype

Annotation of /upstream/dynamips-0.2.7-RC1/frame_relay.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 7 - (hide annotations)
Sat Oct 6 16:23:47 2007 UTC (12 years, 2 months ago) by dpavlin
File MIME type: text/plain
File size: 15356 byte(s)
dynamips-0.2.7-RC1

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

  ViewVC Help
Powered by ViewVC 1.1.26