Remove some accidentally commited test code
[olsrd.git] / lib / quagga / src / quagga.c
1 /*
2  * OLSRd Quagga plugin
3  *
4  * Copyright (C) 2006-2008 Immo 'FaUl' Wehrenberg <immo@chaostreff-dortmund.de>
5  * Copyright (C) 2007-2010 Vasilis Tsiligiannis <acinonyxs@yahoo.gr>
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License version 2 as
9  * published by the Free Software Foundation or - at your option - under
10  * the terms of the GNU General Public Licence version 2 but can be
11  * linked to any BSD-Licenced Software with public available sourcecode
12  *
13  */
14
15 /* -------------------------------------------------------------------------
16  * File               : quagga.c
17  * Description        : functions to interface to the zebra daemon
18  * ------------------------------------------------------------------------- */
19
20 #define HAVE_SOCKLEN_T
21
22 #include "quagga.h"
23 #include "olsr.h" /* olsr_exit
24                      olsr_malloc */
25 #include "log.h" /* olsr_syslog */
26
27 #include <stdint.h>
28 #include <stdlib.h>
29 #include <unistd.h>
30 #include <string.h>
31 #include <fcntl.h>
32 #include <errno.h>
33 #include <sys/types.h>
34 #include <sys/socket.h>
35 #include <netinet/in.h>
36 #include <arpa/inet.h>
37 #include <sys/un.h>
38
39 /* prototypes intern */
40 static struct {
41   char status;                         // internal status
42   char options;                        // internal options
43   int sock;                            // Socket to zebra...
44   char redistribute[ZEBRA_ROUTE_MAX];
45   char distance;
46   char flags;
47   char *sockpath;
48   unsigned int port;
49   char version;
50 } zebra;
51
52 static void *my_realloc(void *, size_t, const char *);
53 static void zebra_connect(void);
54 static unsigned char *try_read(ssize_t *);
55 static int zebra_send_command(unsigned char *);
56 static unsigned char *zebra_route_packet(uint16_t, struct zebra_route *);
57 static unsigned char *zebra_redistribute_packet(uint16_t, unsigned char);
58 static void zebra_enable_redistribute(void);
59 static void zebra_disable_redistribute(void);
60 static struct zebra_route *zebra_parse_route(unsigned char *);
61 static void zebra_reconnect(void);
62 static void free_ipv4_route(struct zebra_route *);
63
64 static void *
65 my_realloc(void *buf, size_t s, const char *c)
66 {
67   buf = realloc(buf, s);
68   if (!buf) {
69     OLSR_PRINTF(1, "(QUAGGA) OUT OF MEMORY: %s\n", strerror(errno));
70     olsr_syslog(OLSR_LOG_ERR, "olsrd: out of memory!: %m\n");
71     olsr_exit(c, EXIT_FAILURE);
72   }
73   return buf;
74 }
75
76 void
77 init_zebra(void)
78 {
79
80   memset(&zebra, 0, sizeof zebra);
81   zebra.sockpath = olsr_malloc(sizeof ZEBRA_SOCKPATH  + 1, "zebra_sockpath");
82   strscpy(zebra.sockpath, ZEBRA_SOCKPATH, sizeof ZEBRA_SOCKPATH);
83
84 }
85
86 void
87 zebra_cleanup(void)
88 {
89   struct rt_entry *tmp;
90
91   if (zebra.options & OPTION_EXPORT) {
92     OLSR_FOR_ALL_RT_ENTRIES(tmp) {
93       zebra_del_route(tmp);
94     }
95     OLSR_FOR_ALL_RT_ENTRIES_END(tmp);
96   }
97   zebra_disable_redistribute();
98
99 }
100
101 static void
102 zebra_reconnect(void)
103 {
104   struct rt_entry *tmp;
105
106   zebra_connect();
107   if (!(zebra.status & STATUS_CONNECTED))
108     return;                     // try again next time
109
110   if (zebra.options & OPTION_EXPORT) {
111     OLSR_FOR_ALL_RT_ENTRIES(tmp) {
112       zebra_add_route(tmp);
113     }
114     OLSR_FOR_ALL_RT_ENTRIES_END(tmp);
115   }
116   zebra_enable_redistribute();
117
118 }
119
120 /* Connect to the zebra-daemon, returns a socket */
121 static void
122 zebra_connect(void)
123 {
124
125   int ret;
126   union {
127     struct sockaddr_in sin;
128     struct sockaddr_un sun;
129   } sockaddr;
130
131   if (close(zebra.sock) < 0)
132     olsr_exit("(QUAGGA) Could not close socket!", EXIT_FAILURE);
133   zebra.sock = socket(zebra.port ? AF_INET : AF_UNIX, SOCK_STREAM, 0);
134
135   if (zebra.sock < 0)
136     olsr_exit("(QUAGGA) Could not create socket!", EXIT_FAILURE);
137
138   memset(&sockaddr, 0, sizeof sockaddr);
139
140   if (zebra.port) {
141     sockaddr.sin.sin_family = AF_INET;
142     sockaddr.sin.sin_port = htons(zebra.port);
143     sockaddr.sin.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
144     ret = connect(zebra.sock, (struct sockaddr *)&sockaddr.sin, sizeof sockaddr.sin);
145   } else {
146     sockaddr.sun.sun_family = AF_UNIX;
147     strscpy(sockaddr.sun.sun_path, zebra.sockpath, sizeof(sockaddr.sun.sun_path));
148     ret = connect(zebra.sock, (struct sockaddr *)&sockaddr.sun, sizeof sockaddr.sun);
149   }
150
151   if (ret < 0)
152     zebra.status &= ~STATUS_CONNECTED;
153   else
154     zebra.status |= STATUS_CONNECTED;
155 }
156
157 /* Sends a command to zebra, command is
158    the command defined in zebra.h, options is the packet-payload,
159    optlen the length, of the payload */
160 static int
161 zebra_send_command(unsigned char *options)
162 {
163
164   unsigned char *pnt;
165   uint16_t len;
166   int ret;
167
168   if (!(zebra.status & STATUS_CONNECTED))
169     return 0;
170
171   pnt = options;
172   memcpy(&len, pnt, 2);
173
174   len = ntohs(len);
175
176   do {
177     ret = write(zebra.sock, pnt, len);
178     if (ret < 0) {
179       if ((errno == EINTR) || (errno == EAGAIN)) {
180         errno = 0;
181         ret = 0;
182         continue;
183       } else {
184         OLSR_PRINTF(1, "(QUAGGA) Disconnected from zebra\n");
185         zebra.status &= ~STATUS_CONNECTED;
186         /* TODO: Remove HNAs added from redistribution */
187         free(options);
188         return -1;
189       }
190     }
191     pnt = pnt + ret;
192   }
193   while ((len -= ret));
194   free(options);
195   return 0;
196 }
197
198 /* Creates a Route-Packet-Payload, needs address, netmask, nexthop,
199    distance, and a pointer of an size_t */
200 static unsigned char *
201 zebra_route_packet(uint16_t cmd, struct zebra_route *r)
202 {
203
204   int count;
205   uint8_t len;
206   uint16_t size;
207   uint32_t ind, metric;
208
209   unsigned char *cmdopt, *t;
210
211   cmdopt = olsr_malloc(ZEBRA_MAX_PACKET_SIZ, "zebra add_v4_route");
212
213   t = &cmdopt[2];
214   if (zebra.version) {
215     *t++ = ZEBRA_HEADER_MARKER;
216     *t++ = zebra.version;
217     cmd = htons(cmd);
218     memcpy(t, &cmd, sizeof cmd);
219     t += sizeof cmd;
220   } else
221       *t++ = (unsigned char) cmd;
222   *t++ = r->type;
223   *t++ = r->flags;
224   *t++ = r->message;
225   *t++ = r->prefixlen;
226   len = (r->prefixlen + 7) / 8;
227   memcpy(t, &r->prefix.v4.s_addr, len);
228   t = t + len;
229
230   if (r->message & ZAPI_MESSAGE_NEXTHOP) {
231     *t++ = r->nexthop_num + r->ifindex_num;
232
233       for (count = 0; count < r->nexthop_num; count++) {
234         *t++ = ZEBRA_NEXTHOP_IPV4;
235         memcpy(t, &r->nexthop[count].v4.s_addr, sizeof r->nexthop[count].v4.s_addr);
236         t += sizeof r->nexthop[count].v4.s_addr;
237       }
238       for (count = 0; count < r->ifindex_num; count++) {
239         *t++ = ZEBRA_NEXTHOP_IFINDEX;
240         ind = htonl(r->ifindex[count]);
241         memcpy(t, &ind, sizeof ind);
242         t += sizeof ind;
243       }
244   }
245   if ((r->message & ZAPI_MESSAGE_DISTANCE) > 0)
246     *t++ = r->distance;
247   if ((r->message & ZAPI_MESSAGE_METRIC) > 0) {
248     metric = htonl(r->metric);
249     memcpy(t, &metric, sizeof metric);
250     t += sizeof metric;
251   }
252   size = htons(t - cmdopt);
253   memcpy(cmdopt, &size, 2);
254
255   return cmdopt;
256 }
257
258 /* Check wether there is data from zebra aviable */
259 void
260 zebra_parse(void *foo __attribute__ ((unused)))
261 {
262   unsigned char *data, *f;
263   uint16_t command;
264   uint16_t length;
265   ssize_t len;
266   struct zebra_route *route;
267
268   if (!(zebra.status & STATUS_CONNECTED)) {
269     zebra_reconnect();
270     return;
271   }
272   data = try_read(&len);
273   if (data) {
274     f = data;
275     do {
276       memcpy(&length, f, sizeof length);
277       length = ntohs (length);
278       if (!length) // something wired happened
279         olsr_exit("(QUAGGA) Zero message length??? ", EXIT_FAILURE);
280       if (zebra.version) {
281         if ((f[2] != ZEBRA_HEADER_MARKER) || (f[3] != zebra.version))
282           olsr_exit("(QUAGGA) Invalid zebra header received!", EXIT_FAILURE);
283         memcpy(&command, &f[4], sizeof command);
284         command = ntohs (command);
285       } else
286           command = f[2];
287       switch (command) {
288         case ZEBRA_IPV4_ROUTE_ADD:
289           route = zebra_parse_route(f);
290           ip_prefix_list_add(&olsr_cnf->hna_entries, &route->prefix, route->prefixlen);
291           free_ipv4_route(route);
292           free(route);
293           break;
294         case ZEBRA_IPV4_ROUTE_DELETE:
295           route = zebra_parse_route(f);
296           ip_prefix_list_remove(&olsr_cnf->hna_entries, &route->prefix, route->prefixlen);
297           free_ipv4_route(route);
298           free(route);
299           break;
300         default:
301           break;
302       }
303       f += length;
304     }
305     while ((f - data) < len);
306     free(data);
307   }
308 }
309
310 // tries to read a packet from zebra_socket
311 // if there is something to read - make sure to read whole packages
312 static unsigned char *
313 try_read(ssize_t * size)
314 {
315   unsigned char *buf;
316   ssize_t bytes, bufsize;
317   uint16_t length, offset;
318   int sockstatus;
319
320   /* initialize variables */
321   buf = NULL;
322   offset = 0;
323   *size = 0;
324   bufsize = 0;
325
326   /* save socket status and set non-blocking for read */
327   sockstatus = fcntl(zebra.sock, F_GETFL);
328   fcntl(zebra.sock, F_SETFL, sockstatus|O_NONBLOCK);
329
330   /* read whole packages */
331   do {
332
333     /* (re)allocate buffer */
334     if (*size == bufsize) {
335       bufsize += BUFSIZE;
336       buf = my_realloc(buf, bufsize, "Zebra try_read");
337     }
338
339     /* read from socket */
340     bytes = read(zebra.sock, buf + *size, bufsize - *size);
341     /* handle broken packet */
342     if (!bytes) {
343       free(buf);
344       return NULL;
345     }
346     /* handle no data available */
347     if (bytes < 0) {
348       /* handle disconnect */
349       if (errno != EAGAIN) {    // oops - we got disconnected
350         OLSR_PRINTF(1, "(QUAGGA) Disconnected from zebra\n");
351         zebra.status &= ~STATUS_CONNECTED;
352         /* TODO: Remove HNAs added from redistribution */
353       }
354       free(buf);
355       return NULL;
356     }
357
358     *size += bytes;
359
360     /* detect zebra packet fragmentation */
361     do {
362       memcpy(&length, buf + offset, sizeof length);
363       length = ntohs(length);
364       offset += length;
365     }
366     while (*size >= (ssize_t) (offset + sizeof length));
367     /* set blocking socket on fragmented packet */
368     if (*size != offset)
369       fcntl(zebra.sock, F_SETFL, sockstatus);
370
371   }
372   while (*size != offset);
373
374   /* restore socket status */
375   fcntl(zebra.sock, F_SETFL, sockstatus);
376
377   return buf;
378 }
379
380 /* Parse an ipv4-route-packet recived from zebra
381  */
382 static struct zebra_route
383 *zebra_parse_route(unsigned char *opt)
384 {
385   struct zebra_route *r;
386   int c;
387   size_t size;
388   uint16_t length;
389   unsigned char *pnt;
390
391   memcpy(&length, opt, sizeof length);
392   length = ntohs (length);
393
394   r = olsr_malloc(sizeof *r, "zebra_parse_route");
395   pnt = (zebra.version ? &opt[6] : &opt[3]);
396   r->type = *pnt++;
397   r->flags = *pnt++;
398   r->message = *pnt++;
399   r->prefixlen = *pnt++;
400   r->prefix.v4.s_addr = 0;
401
402   size = (r->prefixlen + 7) / 8;
403   memcpy(&r->prefix.v4.s_addr, pnt, size);
404   pnt += size;
405
406   switch (zebra.version) {
407     case 0:
408     case 1:
409       if (r->message & ZAPI_MESSAGE_NEXTHOP) {
410         r->nexthop_num = *pnt++;
411         r->nexthop = olsr_malloc((sizeof *r->nexthop) * r->nexthop_num, "quagga: zebra_parse_route");
412         for (c = 0; c < r->nexthop_num; c++) {
413           memcpy(&r->nexthop[c].v4.s_addr, pnt, sizeof r->nexthop[c].v4.s_addr);
414           pnt += sizeof r->nexthop[c].v4.s_addr;
415         }
416       }
417
418       if (r->message & ZAPI_MESSAGE_IFINDEX) {
419         r->ifindex_num = *pnt++;
420         r->ifindex = olsr_malloc(sizeof(uint32_t) * r->ifindex_num, "quagga: zebra_parse_route");
421         for (c = 0; c < r->ifindex_num; c++) {
422           memcpy(&r->ifindex[c], pnt, sizeof r->ifindex[c]);
423           r->ifindex[c] = ntohl (r->ifindex[c]);
424           pnt += sizeof r->ifindex[c];
425         }
426       }
427       break;
428     default:
429       OLSR_PRINTF(1, "(QUAGGA) Unsupported zebra packet version!\n");
430       break;
431   }
432
433   if (r->message & ZAPI_MESSAGE_DISTANCE) {
434     r->distance = *pnt++;
435   }
436
437 // Quagga v0.98.6 BUG workaround: metric is always sent by zebra
438 // even without ZAPI_MESSAGE_METRIC message.
439 //  if (r.message & ZAPI_MESSAGE_METRIC) {
440     memcpy(&r->metric, pnt, sizeof (uint32_t));
441     r->metric = ntohl(r->metric);
442     pnt += sizeof r->metric;
443 //  }
444
445   if (pnt - opt != length) {
446     olsr_exit("(QUAGGA) length does not match ??? ", EXIT_FAILURE);
447   }
448
449   return r;
450 }
451
452 static unsigned char
453 *zebra_redistribute_packet (uint16_t cmd, unsigned char type)
454 {
455   unsigned char *data, *pnt;
456   uint16_t size;
457
458   data = olsr_malloc(ZEBRA_MAX_PACKET_SIZ , "zebra_redistribute_packet");
459
460   pnt = &data[2];
461   if (zebra.version) {
462     *pnt++ = ZEBRA_HEADER_MARKER;
463     *pnt++ = zebra.version;
464     cmd = htons(cmd);
465     memcpy(pnt, &cmd, sizeof cmd);
466     pnt += sizeof cmd;
467   } else
468       *pnt++ = (unsigned char) cmd;
469   *pnt++ = type;
470   size = htons(pnt - data);
471   memcpy(data, &size, 2);
472
473   return data;
474 }
475
476 /* start redistribution FROM zebra */
477 int
478 zebra_redistribute(unsigned char type)
479 {
480
481   if (type > ZEBRA_ROUTE_MAX - 1)
482     return -1;
483   zebra.redistribute[type] = 1;
484
485   return 0;
486
487 }
488
489 /* start redistribution FROM zebra */
490 static void
491 zebra_enable_redistribute(void)
492 {
493   unsigned char type;
494
495   for (type = 0; type < ZEBRA_ROUTE_MAX; type++)
496     if (zebra.redistribute[type]) {
497       if (zebra_send_command(zebra_redistribute_packet(ZEBRA_REDISTRIBUTE_ADD, type)) < 0)
498         olsr_exit("(QUAGGA) could not send redistribute add command", EXIT_FAILURE);
499     }
500
501 }
502
503 /* end redistribution FROM zebra */
504 static void
505 zebra_disable_redistribute(void)
506 {
507   unsigned char type;
508
509   for (type = 0; type < ZEBRA_ROUTE_MAX; type++)
510     if (zebra.redistribute[type]) {
511       if (zebra_send_command(zebra_redistribute_packet(ZEBRA_REDISTRIBUTE_DELETE, type)) < 0)
512         olsr_exit("(QUAGGA) could not send redistribute delete command", EXIT_FAILURE);
513     }
514
515 }
516
517 static void
518 free_ipv4_route(struct zebra_route *r)
519 {
520
521   if(r->ifindex_num)
522     free(r->ifindex);
523   if(r->nexthop_num)
524     free(r->nexthop);
525
526 }
527
528 int
529 zebra_add_route(const struct rt_entry *r)
530 {
531
532   struct zebra_route route;
533   int retval;
534
535   route.distance = 0;
536   route.type = ZEBRA_ROUTE_OLSR;
537   route.flags = zebra.flags;
538   route.message = ZAPI_MESSAGE_NEXTHOP | ZAPI_MESSAGE_METRIC;
539   route.prefixlen = r->rt_dst.prefix_len;
540   route.prefix.v4.s_addr = r->rt_dst.prefix.v4.s_addr;
541   route.ifindex_num = 0;
542   route.ifindex = NULL;
543   route.nexthop_num = 0;
544   route.nexthop = NULL;
545
546   if (r->rt_best->rtp_nexthop.gateway.v4.s_addr == r->rt_dst.prefix.v4.s_addr && route.prefixlen == 32) {
547     return 0;                   /* Quagga BUG workaround: don't add routes with destination = gateway
548                                    see http://lists.olsr.org/pipermail/olsr-users/2006-June/001726.html */
549     route.ifindex_num++;
550     route.ifindex = olsr_malloc(sizeof *route.ifindex, "zebra_add_route");
551     *route.ifindex = r->rt_best->rtp_nexthop.iif_index;
552   } else {
553     route.nexthop_num++;
554     route.nexthop = olsr_malloc(sizeof *route.nexthop, "zebra_add_route");
555     route.nexthop->v4.s_addr = r->rt_best->rtp_nexthop.gateway.v4.s_addr;
556   }
557
558   route.metric = r->rt_best->rtp_metric.hops;
559
560   if (zebra.distance) {
561     route.message |= ZAPI_MESSAGE_DISTANCE;
562     route.distance = zebra.distance;
563   }
564
565   retval = zebra_send_command(zebra_route_packet(ZEBRA_IPV4_ROUTE_ADD, &route));
566
567   return retval;
568 }
569
570 int
571 zebra_del_route(const struct rt_entry *r)
572 {
573
574   struct zebra_route route;
575   int retval;
576   route.distance = 0;
577   route.type = ZEBRA_ROUTE_OLSR;
578   route.flags = zebra.flags;
579   route.message = ZAPI_MESSAGE_NEXTHOP | ZAPI_MESSAGE_METRIC;
580   route.prefixlen = r->rt_dst.prefix_len;
581   route.prefix.v4.s_addr = r->rt_dst.prefix.v4.s_addr;
582   route.ifindex_num = 0;
583   route.ifindex = NULL;
584   route.nexthop_num = 0;
585   route.nexthop = NULL;
586
587   if (r->rt_nexthop.gateway.v4.s_addr == r->rt_dst.prefix.v4.s_addr && route.prefixlen == 32) {
588     return 0;                   /* Quagga BUG workaround: don't delete routes with destination = gateway
589                                    see http://lists.olsr.org/pipermail/olsr-users/2006-June/001726.html */
590     route.ifindex_num++;
591     route.ifindex = olsr_malloc(sizeof *route.ifindex, "zebra_del_route");
592     *route.ifindex = r->rt_nexthop.iif_index;
593   } else {
594     route.nexthop_num++;
595     route.nexthop = olsr_malloc(sizeof *route.nexthop, "zebra_del_route");
596     route.nexthop->v4.s_addr = r->rt_nexthop.gateway.v4.s_addr;
597   }
598
599   route.metric = 0;
600
601   if (zebra.distance) {
602     route.message |= ZAPI_MESSAGE_DISTANCE;
603     route.distance = zebra.distance;
604   }
605
606   retval = zebra_send_command(zebra_route_packet(ZEBRA_IPV4_ROUTE_DELETE, &route));
607
608   return retval;
609 }
610
611 void
612 zebra_olsr_distance(unsigned char dist)
613 {
614   zebra.distance = dist;
615 }
616
617 void
618 zebra_olsr_localpref(void)
619 {
620   zebra.flags &= ZEBRA_FLAG_SELECTED;
621 }
622
623 void
624 zebra_export_routes(unsigned char t)
625 {
626   if (t)
627     zebra.options |= OPTION_EXPORT;
628   else
629     zebra.options &= ~OPTION_EXPORT;
630 }
631
632 void
633 zebra_sockpath(char *sockpath)
634 {
635   size_t len;
636
637   len = strlen(sockpath) + 1;
638   zebra.sockpath = my_realloc(zebra.sockpath, len, "zebra_sockpath");
639   memcpy(zebra.sockpath, sockpath, len);
640
641 }
642
643 void
644 zebra_port(unsigned int port)
645 {
646
647   zebra.port = port;
648
649 }
650
651 void
652 zebra_version(char version)
653 {
654
655   zebra.version = version;
656
657 }
658
659 /*
660  * Local Variables:
661  * c-basic-offset: 2
662  * indent-tabs-mode: nil
663  * End:
664  */