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