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