/[dynamips]/upstream/dynamips-0.2.5/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 /upstream/dynamips-0.2.5/frame_relay.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 1 - (show annotations)
Sat Oct 6 16:01:44 2007 UTC (12 years, 1 month ago) by dpavlin
File MIME type: text/plain
File size: 15365 byte(s)
import 0.2.5 from upstream

1 /*
2 * Cisco 7200 (Predator) 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 /* 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