b79cc3e4ef47fa1b1b4ba8c7d0961e4f0d4b4a12
[olsrd.git] / lib / quagga / test / quagga.try1.c
1 /* 
2  *  (C) 2006 by Immo 'FaUl' Wehrenberg <immo@chaostreff-dortmund.de>
3  *  
4  *  This code is covered by the GPLv2
5  *   
6  */
7
8 #include <stdint.h>
9 #ifdef MY_DEBUG
10 #include <stdio.h>
11 #endif
12 #include <sys/types.h>
13 #include <sys/socket.h>
14 #include <netinet/in.h>
15 #define HAVE_SOCKLEN_T
16 #include <quagga/zebra.h>
17 #include "quagga.h"
18
19 #ifdef OLSR_PLUGIN
20 #include "olsr.h"
21 #include "log.h"
22 #include "defs.h"
23 #include "local_hna_set.h"
24 #endif
25
26 #define ZAPI_MESSAGE_NEXTHOP  0x01
27 #define ZAPI_MESSAGE_IFINDEX  0x02
28 #define ZAPI_MESSAGE_DISTANCE 0x04
29 #define ZAPI_MESSAGE_METRIC   0x08
30
31
32 #define STATUS_CONNECTED 1
33 #define BUFSIZE 1024
34 static char status = 0;
35
36 static int zsock; // Socket to zebra...
37 struct ipv4_route *quagga_routes = 0; // routes currently exportet to zebra
38
39
40 /* prototypes ntern */
41 static char *try_read (ssize_t *);
42 static char* zebra_route_packet (struct ipv4_route r, ssize_t *);
43 static int parse_interface_add (char *, size_t);
44 static int parse_interface_delete (char *, size_t);
45 static int parse_interface_up (char *, size_t);
46 static int parse_interface_down (char *, size_t);
47 static int parse_interface_address_add (char *, size_t);
48 static int parse_interface_address_delete (char *, size_t);
49 static int parse_ipv4_route (char *, size_t, struct ipv4_route *);
50 static int ipv4_route_add (char *, size_t);
51 static int ipv4_route_delete (char *, size_t);
52 static int parse_ipv6_route_add (char*, size_t);
53 static uint32_t prefixlentomask (uint8_t);
54 static void free_ipv4_route (struct ipv4_route);
55 static void update_olsr_zebra_routes (struct ipv4_route*, struct ipv4_route*);
56 static struct ipv4_route *zebra_create_ipv4_route_table_entry (uint32_t,
57                                                                uint32_t,
58                                                                uint32_t);
59 static struct ipv4_route *zebra_create_ipv4_route_table (void);
60 static void zebra_free_ipv4_route_table (struct ipv4_route*);
61 static uint8_t masktoprefixlen (uint32_t);
62
63
64
65 #ifdef MY_DEBUG
66 static void dump_ipv4_route (struct ipv4_route r, char *c) {
67   int i = 0, x = 0;
68
69   puts (c);
70   printf("type: %d\n", r.type);
71   puts("flags:");
72   printf("  Internal: %s\n",r.flags&ZEBRA_FLAG_INTERNAL?"yes":"no");
73   printf("  Selfroute %s\n",r.flags&ZEBRA_FLAG_SELFROUTE?"yes":"no");
74   printf("  Blackhole %s\n",r.flags&ZEBRA_FLAG_BLACKHOLE?"yes":"no");
75   printf("  IBGP: %s\n",r.flags&ZEBRA_FLAG_IBGP?"yes":"no");
76   printf("  Selected: %s\n",r.flags&ZEBRA_FLAG_SELECTED?"yes":"no");
77   printf("  Changed: %s\n",r.flags&ZEBRA_FLAG_CHANGED?"yes":"no");
78   printf("  static: %s\n",r.flags&ZEBRA_FLAG_STATIC?"yes":"no");
79   printf("  reject: %s\n",r.flags&ZEBRA_FLAG_REJECT?"yes":"no");
80   puts("message:");
81   printf("  nexthop: %s\n",r.message&ZAPI_MESSAGE_NEXTHOP?"yes":"no");
82   printf("  ifindex: %s\n",r.message&ZAPI_MESSAGE_IFINDEX?"yes":"no");
83   printf("  distance: %s\n",r.message&ZAPI_MESSAGE_DISTANCE?"yes":"no");
84   printf("  metric: %s\n",r.message&ZAPI_MESSAGE_METRIC?"yes":"no");
85   printf("Prefixlen: %d\n", r.prefixlen);
86   printf("Prefix: %d", (unsigned char)r.prefix);
87   c = (char*) &r.prefix;
88   while (++i < (r.prefixlen/8 + (r.prefixlen % 8 ? 1 : 0)))
89     printf(".%d",(unsigned char)*(c + i));
90   while (i++ < 4)
91     printf(".0");
92   puts("");
93   i=0;
94   if (r.message&ZAPI_MESSAGE_NEXTHOP) {
95     
96     printf("nexthop-count: %d\n", r.nh_count);
97     while (i++ < r.nh_count) {
98       c = (unsigned char*) &r.nexthops[i];
99       printf ("Nexthop %d: %d", i, (unsigned char) *c);
100       while (++x < 4) {
101         printf (".%d", (unsigned char) c[x]);
102       }
103       puts("");
104     }
105     i=0;
106   }
107   if (r.message&ZAPI_MESSAGE_IFINDEX) {
108     
109     printf("index-number: %d\n", r.ind_num);
110     while (i++ < r.ind_num)
111       printf("Index: %d: %d\n", i, r.index[i]);
112     i=0;
113     if (r.message&ZAPI_MESSAGE_DISTANCE)
114       printf("Distance: %d\n",r.distance);
115     if (r.message&ZAPI_MESSAGE_METRIC)
116       printf("Metric: %d\n",r.metric);
117     puts("\n");
118   }
119 }
120 #endif
121
122 void *my_realloc (void *buf, size_t s, const char *c) {
123   buf = realloc (buf, s);
124   if (!buf) {
125 #ifdef OLSR_PLUGIN
126     OLSR_PRINTF (1, "OUT OF MEMORY: %s\n", strerror(errno));
127     olsr_syslog(OLSR_LOG_ERR, "olsrd: out of memory!: %m\n");
128     olsr_exit(c, EXIT_FAILURE);
129 #else
130     exit (EXIT_FAILURE);
131 #endif
132   }
133   return buf;
134 }
135
136
137 #ifndef OLSR_PLUGIN 
138 void *olsr_malloc (size_t f, const char *c) {
139   void* v = malloc (f);
140   return v;
141 }
142 #endif
143
144 /* Connect to the zebra-daemon, returns a socket */
145 int init_zebra () {
146   struct sockaddr_in i;
147   int ret;
148
149   zsock = socket (AF_INET,SOCK_STREAM, 0);
150   if (zsock <0 ) // TODO: Could not create socket
151     return -1;
152   memset (&i, 0, sizeof i);
153   i.sin_family = AF_INET;
154   i.sin_port = htons (ZEBRA_PORT);
155   //  i.sin_len = sizeof i;
156   i.sin_addr.s_addr = htonl (INADDR_LOOPBACK);
157   
158   ret = connect (zsock, (struct sockaddr *)&i, sizeof i);
159   if  (ret < 0) {
160     close (zsock);
161     return -1;
162   }
163   status |= STATUS_CONNECTED;
164   return zsock;
165 }
166
167     
168 /* Sends a command to zebra, command is 
169    the command defined in zebra.h, options is the packet-payload, 
170    optlen the length, of the payload */
171 char zebra_send_command (unsigned char command, char * options, int optlen) {
172
173   char *p = olsr_malloc (optlen+3, "zebra send_command");
174   uint16_t length = optlen + 3;  // length of option + command + packet_length
175   
176   int ret;
177   
178   uint16_t len = htons(length);
179   memcpy (p, &len, 2);
180   p[2] = command;
181   memcpy (p + 3, options, optlen);
182   
183   do {
184     ret = write (zsock, p, length);
185     if (ret < 0) {
186       if (errno == EINTR) continue;
187     }
188     else return -1;
189     p = p+ret;
190   } while ((length -= ret));
191
192   return 0;
193 }
194
195
196 /* Creates a Route-Packet-Payload, needs address, netmask, nexthop, 
197    distance, and a pointer of an size_t */
198 static char* zebra_route_packet (struct ipv4_route r, ssize_t *optlen) {
199
200   char *cmdopt, *t;
201   *optlen = 9; // first: type, flags, message, prefixlen, nexthop number, nexthop)
202   *optlen += r.prefixlen / 8 + (r.prefixlen % 8 ? 1 : 0);
203   
204   cmdopt = olsr_malloc (*optlen, "zebra add_v4_route");
205   t = cmdopt;
206   *t++ = 10; // Type: olsr
207   *t++ = r.flags; // flags
208   *t++ = r.message; // message: contains nexthop
209   *t++ = r.prefixlen;
210   memcpy (t, &r.prefix, r.prefixlen/8 + (r.prefixlen % 8 ? 1 : 0));
211   *t += r.prefixlen/8 + (r.prefixlen % 8 ? 1 : 0);
212   *t++ = r.nh_count;
213   memcpy (t, r.nexthops, r.nh_count * sizeof *r.nexthops);
214   return cmdopt;
215 }
216
217
218 /* adds a route to zebra-daemon (needs socket from zebra,
219    address = prefix of the route
220    mask = netmask of the route
221    nexthop = nexthop of the route
222    distance = distance-value of the route
223 */
224 int zebra_add_v4_route (struct ipv4_route r) {
225   
226   char *cmdopt;
227   ssize_t optlen;
228
229   cmdopt = zebra_route_packet (r, &optlen);
230
231   puts ("DEBUG: zebra_route_packet returned");
232   
233   
234
235   return zebra_send_command (ZEBRA_IPV4_ROUTE_ADD, cmdopt, optlen);
236
237 }
238
239 /* deletes a route from the zebra-daemon (
240    needs socket from zebra,
241    address = prefix of the route
242    mask = netmask of the route
243    nexthop = nexthop of the route
244    distance = distance-value of the route
245 */
246 int zebra_delete_v4_route (struct ipv4_route r) {
247   
248   char *cmdopt;
249   ssize_t optlen;
250   
251   cmdopt = zebra_route_packet (r, &optlen);
252   
253   return zebra_send_command (ZEBRA_IPV4_ROUTE_DELETE, cmdopt, optlen);
254
255 }
256
257
258 /* Check wether there is data from zebra aviable */
259 void zebra_check (void* foo) {
260   char *data, *f;
261   ssize_t len, ret;
262
263   if (!status & STATUS_CONNECTED) {
264   }
265   data = try_read (&len);
266   if (data) {
267     f = data;
268     do {
269       ret = zebra_parse_packet (f, len);
270       if (!ret) {//something wired happened
271         puts ("DEBUG: IIIIIIIIIIRGS");
272         exit (EXIT_FAILURE);
273       }
274       f += ret;
275     } while ((f - data) < len);
276     free (data);
277   }
278 }
279
280
281 // tries to read a packet from zebra_socket
282 // if there is something to read - make sure to read whole packages
283 static char *try_read (ssize_t *len) {
284   char *buf = NULL;
285   ssize_t ret = 0, bsize = 0;
286   uint16_t length = 0, l = 0;
287   int sockstate;
288
289   *len = 0;
290
291   sockstate = fcntl (zsock, F_GETFL, 0);
292   fcntl (zsock, F_SETFL, sockstate|O_NONBLOCK);
293
294   do { 
295     if (*len == bsize) {
296       bsize += BUFSIZE;
297       buf = my_realloc (buf, bsize, "Zebra try_read");
298     }
299     ret = read (zsock, buf + l, bsize - l);
300     if (ret <= 0) {
301       if (errno == EAGAIN) {
302         errno = 0;
303       }
304       else {
305         // TODO: errorhandling
306         ;
307       }
308       free (buf);
309       return NULL;
310     }
311     *len += ret;
312
313     while ((*len - l) > length) {
314       //      printf ("DEBUG: *len -l > length - %d - %d > %d\n", *len, l, length);
315       l += length;
316       memcpy (&length, buf + l, 2);
317       length = ntohs (length);
318     }
319     //    printf ("DEBUG: *len, l, length: %d,%d,%d\n", *len, l, length);
320     if (((*len) - l) == length) break; // GOT FULL PACKAGE!!
321     if (*len < l) {
322       //      printf ("DEBUG: *len, l, length: %d,%d,%d\n", *len, l, length);
323       fcntl (zsock, F_SETFL, sockstate);
324       continue;
325     }
326   } while (1);
327
328   fcntl (zsock, F_SETFL, sockstate);
329   return buf;
330 }
331
332
333 /* Parse a packet recived from zebra */
334 int zebra_parse_packet (char *packet, ssize_t maxlen) {
335
336   /* Array of functions */
337   int (*foo[ZEBRA_MESSAGE_MAX]) (char *, size_t) = {
338     parse_interface_add,
339     parse_interface_delete,
340     parse_interface_address_add,
341     parse_interface_address_delete,
342     parse_interface_up,
343     parse_interface_down,
344     ipv4_route_add,
345     ipv4_route_delete,
346     parse_ipv6_route_add
347   };
348   
349   puts ("DEBUG: zebra_parse_packet");
350   uint16_t length;
351   
352   int ret;
353   memcpy (&length, packet, 2);
354   length = ntohs (length);
355   
356   if (maxlen < length) {
357     puts("Error: programmer is an idiot");
358     printf ("DEBUG: maxlen = %d, packet_length = %d\n", maxlen, length);
359     return maxlen;
360   }
361
362   if (packet[2] - 1 < ZEBRA_MESSAGE_MAX && foo[packet[2] - 1]) { 
363     if (!(ret = foo[packet[2] - 1] (packet + 3, length - 3))) 
364       return length;
365     else printf ("DEBUG: Parse error: %d\n", ret);
366   }
367   else
368     printf ("Unknown packet type: %d\n", packet[2]);
369
370   puts ("Quagga: RECIVED PACKET FROM ZEBRA THAT I CAN'T PARSE");
371
372   return length;
373 }
374
375
376 static int parse_interface_add (char *opt, size_t len) {
377   //todo
378   return 0;
379 }
380
381
382 static int parse_interface_delete (char *opt, size_t len) {
383   //todo
384   return 0;
385 }
386
387
388 static int parse_interface_address_add (char *opt, size_t len) {
389   
390   //todo
391   return 0;
392 }
393
394 static int parse_interface_up (char *opt, size_t len) {
395   
396   //todo
397   return 0;
398 }
399
400 static int parse_interface_down (char *opt, size_t len) {
401   
402   //todo
403   return 0;
404 }
405
406
407 static int parse_interface_address_delete (char *opt, size_t  len) {
408   //todo
409   return 0;
410 }
411
412
413 /* Parse an ipv4-route-packet recived from zebra
414  */
415 static int parse_ipv4_route (char *opt, size_t len, struct ipv4_route *r) {
416   //  puts ("DEBUG: parse_ipv4_route");
417   if (len < 4) return -1;
418   
419   r->type = *opt++;
420   r->flags = *opt++;
421   r->message = *opt++;
422   r->prefixlen = *opt++;
423   len -= 4;
424   r->prefix = 0;
425   
426   if ((int)len < r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0)) return -1;
427   
428   memcpy (&r->prefix, opt, r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0));
429   opt += r->prefixlen/8 + (r->prefixlen % 8 ? 1 : 0);
430   
431   if (r->message & ZAPI_MESSAGE_NEXTHOP) {
432     if (len < 1) return -1;
433     r->nh_count = *opt++;
434     if (len < sizeof (uint32_t) * r->nh_count) return -1;
435     r->nexthops = olsr_malloc (sizeof (uint32_t) * r->nh_count, 
436                                "quagga: parse_ipv4_route_add");
437     memcpy (r->nexthops, opt, sizeof (uint32_t) * r->nh_count);
438     opt += sizeof (uint32_t) * r->nh_count;
439     len -= sizeof (uint32_t) * r->nh_count + 1;
440   }
441
442   if (r->message & ZAPI_MESSAGE_IFINDEX) {
443     if (len < 1) return -2;
444     r->ind_num = *opt++;
445     if (len < sizeof (uint32_t) * r->ind_num) return -3;
446     r->index = olsr_malloc (sizeof (uint32_t) * r->ind_num,
447                             "quagga: parse_ipv4_route_add");
448     memcpy (r->index, opt, r->ind_num * sizeof (uint32_t));
449     opt += sizeof (uint32_t) * r->ind_num;
450     len -= sizeof (uint32_t) * r->ind_num;
451   }
452
453   if (r->message & ZAPI_MESSAGE_DISTANCE)
454     // todo
455     ;
456
457   if (r->message & ZAPI_MESSAGE_METRIC) {
458     if (len < sizeof (uint32_t)) return -4;
459     memcpy (&r->metric, opt, sizeof (uint32_t));
460   }
461
462   return 0;
463 }
464
465
466 static int ipv4_route_add (char *opt, size_t len) {
467
468   struct ipv4_route r;
469   int f;
470   
471   //  puts ("DEBUG: ipv4_route_add");
472   
473   f = parse_ipv4_route (opt, len, &r);
474   if (f < 0) {
475     printf ("parse-error: %d\n",f);
476     return f;
477   }
478   
479   add_hna4_route (r);
480   return 0;
481 }
482
483 static int ipv4_route_delete (char *opt, size_t len) {
484   struct ipv4_route r;
485   int f;
486   
487   f = parse_ipv4_route (opt, len, &r);
488   if (f < 0) return f;
489
490   return delete_hna4_route (r);
491   // OK, now delete that foo
492   
493 }
494
495 static int parse_ipv6_route_add (char *opt, size_t len) {
496   //todo
497   return 0;
498 }
499
500
501 /* start redistribution FROM zebra */
502 int zebra_redistribute (unsigned char type) {
503
504   return zebra_send_command (ZEBRA_REDISTRIBUTE_ADD, &type, 1);
505   
506   
507 }  
508
509
510 /* end redistribution FROM zebra */
511 int zebra_disable_redistribute (unsigned char type) {
512   
513   return zebra_send_command (ZEBRA_REDISTRIBUTE_DELETE, &type, 1);
514
515 }
516   
517 static uint32_t prefixlentomask (uint8_t prefix) {
518   uint32_t mask;
519   mask = 0xffffffff<<(32-prefix);
520   mask = ntohl(mask);
521   return mask;
522 }
523
524 int add_hna4_route (struct ipv4_route r) {
525   union olsr_ip_addr net, mask;
526   
527 #ifdef MY_DEBUG
528   dump_ipv4_route(r, "add_hna4_route");
529 #endif
530
531   mask.v4 = prefixlentomask(r.prefixlen);
532   net.v4 = r.prefix;
533
534
535 #ifdef OLSR_PLUGIN
536   add_local_hna4_entry(&net, &mask);
537 #endif
538   free_ipv4_route(r);
539   return 0;
540 }
541
542 int delete_hna4_route (struct ipv4_route r) {
543
544   union olsr_ip_addr net, mask;
545
546 #ifdef MY_DEBUG
547   dump_ipv4_route(r, "delete_hna4_route");
548 #endif
549
550   mask.v4 = prefixlentomask(r.prefixlen);
551   net.v4 = r.prefix;
552
553 #ifdef OLSR_PLUGIN
554   return remove_local_hna4_entry(&net, &mask) ? 0 : -1;
555 #endif
556   
557   free_ipv4_route(r);
558   return 0;
559
560 }
561
562 static void free_ipv4_route (struct ipv4_route r) {
563
564   if(r.message&ZAPI_MESSAGE_IFINDEX && r.ind_num) free(r.index);
565   if(r.message&ZAPI_MESSAGE_NEXTHOP && r.nh_count) free(r.nexthops);
566
567 }
568  
569 void zebra_clear_routes(void) {
570
571   struct ipv4_route *t;
572
573   t = quagga_routes;
574   while (t) {
575     zebra_delete_v4_route(*t);
576     t=t->next;
577   }
578   zebra_free_ipv4_route_table(quagga_routes);
579
580   quagga_routes = NULL;
581 }
582
583
584 void zebra_update_hna (void* f) {
585   
586   struct ipv4_route *a = zebra_create_ipv4_route_table();
587   update_olsr_zebra_routes(a, quagga_routes);
588   zebra_free_ipv4_route_table(quagga_routes);
589
590   quagga_routes = a;
591
592 }
593
594 static struct ipv4_route *zebra_create_ipv4_route_table (void) {
595
596   struct ipv4_route *r = 0, *t = 0 /* make compiler happy */;
597   int i;
598   struct hna_entry *e;
599   struct hna_net *n;
600
601   for (i = 0; i < HASHSIZE; i++) {
602     e = hna_set[i].next;
603     for(;e != &hna_set[i];e = e->next) {
604       n = e->networks.next;
605       for(;n != &e->networks; n = n->next) {
606         if (!r) {
607           r = zebra_create_ipv4_route_table_entry(n->A_network_addr.v4, 
608                                                   n->A_netmask.v4,
609                                                   e->A_gateway_addr.v4);
610           t = r;
611         }
612         else {
613           t->next = zebra_create_ipv4_route_table_entry(n->A_network_addr.v4, 
614                                                         n->A_netmask.v4,
615                                                         e->A_gateway_addr.v4);
616           t = t->next;
617         }
618       }
619     }
620   }
621   
622   return r;
623   
624 }
625
626
627 static struct ipv4_route *zebra_create_ipv4_route_table_entry (uint32_t addr, 
628                                                                uint32_t mask,
629                                                                uint32_t gw) {
630   
631   struct ipv4_route *r;
632   
633
634   r = olsr_malloc (sizeof *r,"zebra_create_ipv4_route_table_entry");
635   memset (r, 0, sizeof *r);
636   r->prefix = addr;
637   r->prefixlen = masktoprefixlen (mask);
638   r->message |= ZAPI_MESSAGE_NEXTHOP;
639   r->nh_count = 1;
640   r->nexthops = olsr_malloc (sizeof (uint32_t), "zebra_create_ipv4_route_table_entry"); 
641   *r->nexthops = gw;
642   r->next = NULL;
643
644   return r;
645 }
646
647 static uint8_t masktoprefixlen (uint32_t mask) {
648   
649   
650   uint8_t prefixlen = 0;
651   while (mask & (1 << ++prefixlen && prefixlen < 32);
652   return prefixlen;
653
654 }
655
656 static void update_olsr_zebra_routes (struct ipv4_route *a, 
657                                       struct ipv4_route *r) {
658   
659   struct ipv4_route *t;
660
661   if (!r) {
662     puts("no quagga_routing_table aviable");
663     for (;a;a = a->next) {
664       dump_ipv4_route (*a, "adding this route");
665       //      zebra_add_v4_route(*r);
666     }
667     return;
668   }
669
670   while (a) {
671     for (t = r; t; t = t->next) {
672       if (a->prefix == t->prefix) 
673         if (a->prefixlen == t->prefixlen)
674           if (*a->nexthops == *t->nexthops) {
675             goto foo;
676           }
677     }
678     dump_ipv4_route (*a, "adding this route");
679     //zebra_add_v4_route(*a);
680   foo:
681     a = a->next;
682   }
683
684   while (r) {
685     for (t = a; t; t = t->next) {
686       if (r->prefix == t->prefix) 
687         if (r->prefixlen == t->prefixlen)
688           if (*r->nexthops == *t->nexthops) {
689             goto bar;
690           }
691     }
692     dump_ipv4_route (*r, "deleting this route");
693     //zebra_delete_v4_route(*r);
694   bar:
695     r = r->next;
696   }
697
698 }
699
700
701 static void zebra_free_ipv4_route_table (struct ipv4_route *r) {
702   struct ipv4_route *n;
703   if (!r) return;
704   while ((n = r->next)) {
705     if (r->message & ZAPI_MESSAGE_NEXTHOP) free (r->nexthops);
706     if (r->message & ZAPI_MESSAGE_IFINDEX) free (r->index);
707     free(r);
708     r = n;
709   }
710 }