/[dynamips]/trunk/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

Contents of /trunk/frame_relay.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 12 - (show annotations)
Sat Oct 6 16:45:40 2007 UTC (16 years, 5 months ago) by dpavlin
File MIME type: text/plain
File size: 15125 byte(s)
make working copy

1 /*
2 * Cisco router simulation platform.
3 * 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 if ((len <= sizeof(lmi_ansi_hdr)) ||
63 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 mp_free(p);
442 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 }

  ViewVC Help
Powered by ViewVC 1.1.26