ef90e3a8c49046948abc7244a9e4c01375273921
[olsrd.git] / lib / secure / src / olsrd_secure.c
1
2 /*
3  * Secure OLSR plugin
4  * http://www.olsr.org
5  *
6  * Copyright (c) 2004, Andreas T√łnnesen(andreto@olsr.org)
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or 
10  * without modification, are permitted provided that the following 
11  * conditions are met:
12  *
13  * * Redistributions of source code must retain the above copyright 
14  *   notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above copyright 
16  *   notice, this list of conditions and the following disclaimer in 
17  *   the documentation and/or other materials provided with the 
18  *   distribution.
19  * * Neither the name of olsrd, olsr.org nor the names of its 
20  *   contributors may be used to endorse or promote products derived 
21  *   from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: olsrd_secure.c,v 1.5 2004/11/18 22:00:48 kattemat Exp $
37  */
38
39
40 /*
41  * Dynamic linked library for the olsr.org olsr daemon
42  */
43
44 #include "olsrd_secure.h"
45 #include <stdio.h>
46 #include <string.h>
47 #include <stdlib.h>
48 #include <linux/in_route.h>
49 #include <unistd.h>
50 #include <errno.h>
51 #include <signal.h>
52
53 /* OpenSSL stuff */
54
55 #include <openssl/sha.h>
56 //#include <openssl/evp.h>
57
58
59 /**
60  *Do initialization here
61  *
62  *This function is called by the my_init
63  *function in uolsrd_plugin.c
64  */
65
66 int
67 olsr_plugin_init()
68 {
69   struct interface *ints;
70   int i;
71
72
73   /* Initialize the timestamp database */
74   for(i = 0; i < HASHSIZE; i++)
75     {
76       timestamps[i].next = &timestamps[i];
77       timestamps[i].prev = &timestamps[i];
78     }
79   olsr_printf(1, "Timestamp database initialized\n");
80
81   if(read_key_from_file(KEYFILE) < 0)
82     {
83       olsr_printf(1, "[ENC]Could not read key from file!\nExitting!\n\n");
84       exit(1);
85     }
86
87   /* Register the packet transform function */
88   add_ptf(&add_signature);
89
90   /* register ifchange function */
91   add_ifchgf(&ifchange);
92
93   /* Hijack OLSR socket parser functions */
94   ints = ifs;
95   while(ints)
96     {
97       olsr_printf(1, "[ENC]Hijacking %s socket %d\n", ints->int_name, ints->olsr_socket);
98       fflush(stdout);
99       remove_olsr_socket(ints->olsr_socket, olsr_input);
100       add_olsr_socket(ints->olsr_socket, &packet_parser);
101      
102       /* Reducing maxmessagesize */
103       net_reserve_bufspace(ints, sizeof(struct olsrmsg));
104
105       ints = ints->int_next;
106     }
107
108   /* Register timeout - poll every 2 seconds */
109   olsr_register_scheduler_event(&timeout_timestamps, 2, 0 , NULL);
110
111   return 1;
112 }
113
114 int
115 plugin_ipc_init()
116 {
117   return 1;
118 }
119
120
121 /*
122  * destructor - called at unload
123  */
124 void
125 olsr_plugin_exit()
126 {
127 }
128
129
130
131 /* Mulitpurpose funtion */
132 int
133 plugin_io(int cmd, void *data, size_t size)
134 {
135
136   switch(cmd)
137     {
138     default:
139       return 0;
140     }
141   
142   return 1;
143 }
144
145
146
147 /**
148  *Scheduled event
149  */
150 void
151 olsr_event()
152 {
153
154 }
155
156
157 int
158 ipc_send(char *data, int size)
159 {
160   return 1;
161 }
162
163
164 /* XXX - ToDo */
165 int
166 ifchange(struct interface *ifn, int action)
167 {
168
169   switch(action)
170     {
171     case(IFCHG_IF_ADD):
172       printf("SEC: interface %s added\n\n", ifn->int_name);
173       olsr_printf(1, "[ENC]Hijacking %s socket %d\n", ifn->int_name, ifn->olsr_socket);
174       remove_olsr_socket(ifn->olsr_socket, olsr_input);
175       add_olsr_socket(ifn->olsr_socket, &packet_parser);
176       /* Reducing maxmessagesize */
177       net_reserve_bufspace(ifn, sizeof(struct olsrmsg));
178       break;
179
180     case(IFCHG_IF_REMOVE):
181       printf("SEC: interface %s removed\n\n", ifn->int_name);
182       olsr_printf(1, "[ENC]Removing %s socket %d\n", ifn->int_name, ifn->olsr_socket);
183       remove_olsr_socket(ifn->olsr_socket, &packet_parser);
184       break;
185
186     case(IFCHG_IF_UPDATE):
187       printf("SEC: interface %s updated\n\n", ifn->int_name);
188       break;
189       
190     default:
191       break;
192     }
193   return 0;
194   
195 }
196
197
198 void
199 packet_parser(int fd)
200 {
201   /* sockaddr_in6 is bigger than sockaddr !!!! */
202   struct sockaddr_storage from;
203   size_t fromlen;
204   int cc;
205   union olsr_ip_addr from_addr;
206   union
207   {
208     char        buf[MAXMESSAGESIZE+1];
209     struct      olsr olsr;
210   } inbuf;
211
212
213   for (;;) 
214     {
215       fromlen = sizeof(struct sockaddr_storage);
216
217       cc = recvfrom(fd, &inbuf, sizeof (inbuf), 0, (struct sockaddr *)&from, &fromlen);
218
219       if (cc <= 0) 
220         {
221           if (cc < 0 && errno != EWOULDBLOCK)
222             {
223               olsr_printf(1, "[ENC]error recvfrom: %s", strerror(errno));
224             }
225           break;
226         }
227
228       if(ipversion == AF_INET)
229         {
230           /* IPv4 sender address */
231           memcpy(&from_addr, &((struct sockaddr_in *)&from)->sin_addr.s_addr, ipsize);
232         }
233       else
234         {
235           /* IPv6 sender address */
236           memcpy(&from_addr, &((struct sockaddr_in6 *)&from)->sin6_addr, ipsize);
237         }
238
239       /*
240       olsr_printf(1, "[ENC]message from %s size %d\n",
241                   olsr_ip_to_string(&from_addr),
242                   cc);
243       */
244
245       /* are we talking to ourselves? */
246       if(if_ifwithaddr(&from_addr) != NULL)
247         return;
248
249
250       /*
251        *setting global from addr
252        */
253       //printf("Recieved data on socket %d\n", socknr);
254
255
256       if((olsr_in_if = if_ifwithsock(fd)) == NULL)
257         {
258           olsr_printf(1, "[ENC]Could not find input interface for message from %s size %d\n",
259                       olsr_ip_to_string(&from_addr),
260                       cc);
261           return ;
262         }
263
264       /*
265        * Check for challenge/response messages
266        */
267       check_auth(inbuf.buf, &cc);
268
269       /*
270        * Check signature
271        */
272
273       if(!validate_packet(inbuf.buf, &cc))
274         {
275           olsr_printf(1, "[ENC]Rejecting packet from %s\n", olsr_ip_to_string((union olsr_ip_addr *)&((struct sockaddr_in *)&from)->sin_addr.s_addr));
276           return;
277         }
278
279
280       olsr_printf(1, "[ENC]Packet from %s OK size %d\n", olsr_ip_to_string((union olsr_ip_addr *)&((struct sockaddr_in *)&from)->sin_addr.s_addr), cc);
281
282
283       /* Fix OLSR packet header */
284       inbuf.olsr.olsr_packlen = htons(cc);
285
286       
287       //olsr_printf(1, "Recieved a packet from %s\n", olsr_ip_to_string((union olsr_ip_addr *)&((struct sockaddr_in *)&from)->sin_addr.s_addr));
288
289       //printf("\nCC: %d FROMLEN: %d\n\n", cc, fromlen);
290       if ((ipversion == AF_INET) && (fromlen != sizeof (struct sockaddr_in)))
291         break;
292       else if ((ipversion == AF_INET6) && (fromlen != sizeof (struct sockaddr_in6)))
293         break;
294
295
296       /*
297        * &from - sender
298        * &inbuf.olsr 
299        * cc - bytes read
300        */
301       parse_packet(&inbuf.olsr, cc, olsr_in_if, &from_addr);
302     
303     }
304 }
305
306
307
308 /**
309  * Check a incoming OLSR packet for
310  * challenge/responses.
311  * They need not be verified as they
312  * are signed in the message.
313  *
314  */
315 int
316 check_auth(char *pck, int *size)
317 {
318
319   olsr_printf(3, "[ENC]Checking packet for challenge response message...\n");
320
321   switch(pck[4])
322     {
323     case(TYPE_CHALLENGE):
324       parse_challenge(&pck[4]);
325       break;
326
327     case(TYPE_CRESPONSE):
328       parse_cres(&pck[4]);
329       break;
330
331     case(TYPE_RRESPONSE):
332       parse_rres(&pck[4]);
333       break;
334
335     default:
336       return 0;
337     }
338
339   return 1;
340 }
341
342
343
344 /**
345  * Packet transform function
346  * Build a SHA-1 hash of the original message
347  * + the signature message(-digest) + key
348  *
349  * Then add the signature message to the packet and
350  * increase the size
351  */
352 int
353 add_signature(char *pck, int *size)
354 {
355   struct olsrmsg *msg;
356 #ifdef DEBUG
357   int i, j;
358   char *sigmsg;                                                                                        
359 #endif
360   
361   olsr_printf(2, "[ENC]Adding signature for packet size %d\n", *size);
362   fflush(stdout);
363   
364   msg = (struct olsrmsg *)&pck[*size];
365   /* Update size */
366   ((struct olsr*)pck)->olsr_packlen = htons(*size + sizeof(struct olsrmsg));
367   
368   /* Fill packet header */
369   msg->olsr_msgtype = MESSAGE_TYPE;
370   msg->olsr_vtime = 0;
371   msg->olsr_msgsize = htons(sizeof(struct olsrmsg));
372   memcpy(&msg->originator, main_addr, ipsize);
373   msg->ttl = 1;
374   msg->hopcnt = 0;
375   msg->seqno = htons(get_msg_seqno());
376   
377   /* Fill subheader */
378   msg->sig.type = ONE_CHECKSUM;
379   msg->sig.algorithm = SHA1_INCLUDING_KEY;
380   memset(&msg->sig.reserved, 0, 2);
381   
382   /* Add timestamp */
383   msg->sig.timestamp = htonl(now->tv_sec);
384   olsr_printf(3, "[ENC]timestamp: %d\n", now->tv_sec);
385   
386   /* Set the new size */
387   *size = *size + sizeof(struct olsrmsg);
388   
389   /* Create packet + key cache */
390   /* First the OLSR packet + signature message - digest */
391   memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
392   /* Then the key */
393   memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);
394   
395   /* Create the SHA1 hash */
396   SHA1(checksum_cache, (*size - SIGNATURE_SIZE) + KEYLENGTH, &pck[*size - SIGNATURE_SIZE]);
397
398  
399
400 #ifdef DEBUG
401   olsr_printf(1, "Signature message:\n");
402
403   j = 0;
404   sigmsg = (char *)msg;
405
406   for(i = 0; i < sizeof(struct olsrmsg); i++)
407     {
408       olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
409       j++;
410       if(j == 4)
411         {
412           olsr_printf(1, "\n");
413           j = 0;
414         }
415     }
416 #endif
417
418   olsr_printf(3, "[ENC] Message signed\n");
419
420   return 1;
421 }
422
423
424
425 int
426 validate_packet(char *pck, int *size)
427 {
428   int packetsize;
429   char sha1_hash[SHA_DIGEST_LENGTH];
430   struct olsrmsg *sig;
431   time_t rec_time;
432
433 #ifdef DEBUG
434   int i, j;
435   char *sigmsg;
436 #endif
437
438   /* Find size - signature message */
439   packetsize = *size - sizeof(struct olsrmsg);
440
441   if(packetsize < 4)
442     return 0;
443
444   sig = (struct olsrmsg *)&pck[packetsize];
445
446   //olsr_printf(1, "Size: %d\n", packetsize);
447
448 #ifdef DEBUG
449   olsr_printf(1, "Input message:\n");
450   
451   j = 0;
452   sigmsg = (char *)sig;
453
454   for(i = 0; i < sizeof(struct olsrmsg); i++)
455     {
456       olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
457       j++;
458       if(j == 4)
459         {
460           olsr_printf(1, "\n");
461           j = 0;
462         }
463     }
464 #endif
465
466   /* Sanity check first */
467   if((sig->olsr_msgtype != MESSAGE_TYPE) || 
468      (sig->olsr_vtime) ||
469      (sig->olsr_msgsize != ntohs(sizeof(struct olsrmsg))) ||
470      (sig->ttl != 1) ||
471      (sig->hopcnt != 0))
472     {
473       olsr_printf(1, "[ENC]Packet not sane!\n");
474       return 0;
475     }
476
477   /* Check scheme and type */
478   switch(sig->sig.type)
479     {
480     case(ONE_CHECKSUM):
481       switch(sig->sig.algorithm)
482         {
483         case(SHA1_INCLUDING_KEY):
484           goto one_checksum_SHA; /* Ahhh... fix this */
485           break;
486           
487         }
488       break;
489
490     default:
491       olsr_printf(1, "[ENC]Unsupported sceme: %d enc: %d!\n", sig->sig.type, sig->sig.algorithm);
492       return 0;
493       break;
494
495     }
496   //olsr_printf(1, "Packet sane...\n");
497
498  one_checksum_SHA:
499
500   /* Create packet + key cache */
501   /* First the OLSR packet + signature message - digest */
502   memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
503   /* Then the key */
504   memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);
505
506
507   /* generate SHA-1 */
508   SHA1(checksum_cache, *size - SIGNATURE_SIZE + KEYLENGTH, sha1_hash);
509
510
511 #ifdef DEBUG
512   olsr_printf(1, "Recevied hash:\n");
513   
514   sigmsg = (char *)sig->sig.signature;
515
516   for(i = 0; i < 20; i++)
517     {
518       olsr_printf(1, " %3i", (u_char) sigmsg[i]);
519     }
520   olsr_printf(1, "\n");
521
522   olsr_printf(1, "Calculated hash:\n");
523   
524   sigmsg = sha1_hash;
525
526   for(i = 0; i < 20; i++)
527     {
528       olsr_printf(1, " %3i", (u_char) sigmsg[i]);
529     }
530   olsr_printf(1, "\n");
531 #endif
532
533   if(memcmp(sha1_hash, sig->sig.signature, 20) != 0)
534     {
535       olsr_printf(1, "[ENC]Signature missmatch\n");
536       return 0;
537     }
538
539   /* Check timestamp */
540   rec_time = ntohl(sig->sig.timestamp);
541
542   if(!check_timestamp((union olsr_ip_addr *)&sig->originator, rec_time))
543     {
544       olsr_printf(1, "[ENC]Timestamp missmatch in packet from %s!\n",
545                   olsr_ip_to_string((union olsr_ip_addr *)&sig->originator));
546       return 0;
547     }
548
549   olsr_printf(1, "[ENC]Received timestamp %d diff: %d\n", rec_time, now->tv_sec - rec_time);
550
551   /* Remove signature message */
552   *size = packetsize;
553   return 1;
554 }
555
556
557 int
558 check_timestamp(union olsr_ip_addr *originator, time_t tstamp)
559 {
560   struct stamp *entry;
561   int diff;
562
563   entry = lookup_timestamp_entry(originator);
564
565   if(!entry)
566     {
567       /* Initiate timestamp negotiation */
568
569       send_challenge(originator);
570
571       return 0;
572     }
573
574   if(!entry->validated)
575     {
576       olsr_printf(1, "[ENC]Message from non-validated host!\n");
577       return 0;
578     }
579
580   diff = entry->diff - (now->tv_sec - tstamp);
581
582   olsr_printf(3, "[ENC]Timestamp slack: %d\n", diff);
583
584   if((diff > UPPER_DIFF) || (diff < LOWER_DIFF))
585     {
586       olsr_printf(1, "[ENC]Timestamp scew detected!!\n");
587       return 0;
588     }
589
590   /* ok - update diff */
591   entry->diff = ((now->tv_sec - tstamp) + entry->diff) ?
592     ((now->tv_sec - tstamp) + entry->diff) / 2 : 0;
593
594   olsr_printf(3, "[ENC]Diff set to : %d\n", entry->diff);
595
596   /* update validtime */
597   olsr_get_timestamp((olsr_u32_t) TIMESTAMP_HOLD_TIME*1000, &entry->valtime);
598
599   return 1;
600 }
601
602
603 /**
604  * Create and send a timestamp
605  * challenge message to new_host
606  *
607  * The host is registered in the timestamps
608  * repository with valid=0
609  */
610
611 int
612 send_challenge(union olsr_ip_addr *new_host)
613 {
614   struct challengemsg cmsg;
615   struct stamp *entry;
616   olsr_u32_t challenge, hash;
617
618   olsr_printf(1, "[ENC]Building CHALLENGE message\n");
619
620   /* Set the size including OLSR packet size */
621
622
623   challenge = rand() << 16;
624   challenge |= rand();
625
626   /* Fill challengemessage */
627   cmsg.olsr_msgtype = TYPE_CHALLENGE;
628   cmsg.olsr_vtime = 0;
629   cmsg.olsr_msgsize = htons(sizeof(struct challengemsg));
630   memcpy(&cmsg.originator, main_addr, ipsize);
631   cmsg.ttl = 1;
632   cmsg.hopcnt = 0;
633   cmsg.seqno = htons(get_msg_seqno());
634
635   /* Fill subheader */
636   memcpy(&cmsg.destination, new_host, ipsize);
637   cmsg.challenge = htonl(challenge);
638
639   olsr_printf(3, "[ENC]Size: %d\n", sizeof(struct challengemsg));
640
641   /* Create packet + key cache */
642   /* First the OLSR packet + signature message - digest */
643   memcpy(checksum_cache, &cmsg, sizeof(struct challengemsg) - SIGNATURE_SIZE);
644   /* Then the key */
645   memcpy(&checksum_cache[sizeof(struct challengemsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
646
647   /* Create the SHA1 hash */
648   SHA1(checksum_cache, 
649        (sizeof(struct challengemsg) - SIGNATURE_SIZE) + KEYLENGTH, 
650        cmsg.signature);
651
652   olsr_printf(3, "[ENC]Sending timestamp request to %s challenge 0x%x\n", 
653               olsr_ip_to_string(new_host),
654               challenge);
655
656   /* Add to buffer */
657   net_outbuffer_push(olsr_in_if, (olsr_u8_t *)&cmsg, sizeof(struct challengemsg));
658
659   /* Send the request */
660   net_output(olsr_in_if);
661
662   /* Create new entry */
663   entry = malloc(sizeof(struct stamp));
664   
665   entry->diff = 0;
666   entry->validated = 0;
667   entry->challenge = challenge;
668
669   memcpy(&entry->addr, new_host, ipsize);
670
671   /* update validtime - not validated */
672   olsr_get_timestamp((olsr_u32_t) EXCHANGE_HOLD_TIME*1000, &entry->conftime);
673   
674   hash = olsr_hashing(new_host);
675   
676   /* Queue */
677   timestamps[hash].next->prev = entry;
678   entry->next = timestamps[hash].next;
679   timestamps[hash].next = entry;
680   entry->prev = &timestamps[hash];
681
682
683   return 1;
684
685 }
686
687 int
688 parse_cres(char *in_msg)
689 {
690   struct c_respmsg *msg;
691   char sha1_hash[20];
692   struct stamp *entry;
693
694   msg = (struct c_respmsg *)in_msg;
695
696   olsr_printf(1, "[ENC]Challenge-response message received\n");
697   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
698
699   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
700     {
701       olsr_printf(3, "[ENC]Not for us...\n");
702       return 0;
703     }
704
705   olsr_printf(3, "[ENC]Challenge: 0x%x\n", ntohl(msg->challenge));
706
707   /* Check signature */
708
709   /* Create packet + key cache */
710   /* First the OLSR packet + signature message - digest */
711   memcpy(checksum_cache, msg, sizeof(struct c_respmsg) - SIGNATURE_SIZE);
712   /* Then the key */
713   memcpy(&checksum_cache[sizeof(struct c_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
714   
715   /* Create the SHA1 hash */
716   SHA1(checksum_cache, 
717        (sizeof(struct c_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
718        sha1_hash);
719   
720   if(memcmp(sha1_hash, &msg->signature, 20) != 0)
721     {
722       olsr_printf(1, "[ENC]Signature missmatch in challenge-response!\n");
723       return 0;
724     }
725
726   olsr_printf(3, "[ENC]Signature verified\n");
727
728
729   /* Now to check the digest from the emitted challenge */
730   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
731     {
732       olsr_printf(1, "[ENC]Received challenge-response from non-registered node %s!\n",
733                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
734       return 0;
735     }
736
737   /* Generate the digest */
738   olsr_printf(3, "[ENC]Entry-challenge 0x%x\n", entry->challenge);
739
740   /* First the challenge received */
741   memcpy(checksum_cache, &entry->challenge, 4);
742   /* Then the local IP */
743   memcpy(&checksum_cache[sizeof(olsr_u32_t)], &msg->originator, ipsize);
744
745   /* Create the SHA1 hash */
746   SHA1(checksum_cache, 
747        sizeof(olsr_u32_t) + ipsize, 
748        sha1_hash);
749
750
751   if(memcmp(msg->res_sig, sha1_hash, 20) != 0)
752     {
753       olsr_printf(1, "[ENC]Error in challenge signature from %s!\n",
754                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
755       
756       return 0;
757     }
758
759   olsr_printf(3, "[ENC]Challenge-response signature ok\n");
760
761   /* Update entry! */
762
763
764   entry->challenge = 0;
765   entry->validated = 1;
766   entry->diff = now->tv_sec - msg->timestamp;
767
768   /* update validtime - validated entry */
769   olsr_get_timestamp((olsr_u32_t) TIMESTAMP_HOLD_TIME*1000, &entry->valtime);
770
771   olsr_printf(1, "[ENC]%s registered with diff %d!\n",
772               olsr_ip_to_string((union olsr_ip_addr *)&msg->originator),
773               entry->diff);
774
775   /* Send response-response */
776   send_rres((union olsr_ip_addr *)&msg->originator, 
777             (union olsr_ip_addr *)&msg->destination, 
778             ntohl(msg->challenge));
779
780   return 1;
781 }
782
783
784 int
785 parse_rres(char *in_msg)
786 {
787   struct r_respmsg *msg;
788   char sha1_hash[20];
789   struct stamp *entry;
790
791   msg = (struct r_respmsg *)in_msg;
792
793   olsr_printf(1, "[ENC]Response-response message received\n");
794   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
795
796   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
797     {
798       olsr_printf(1, "[ENC]Not for us...\n");
799       return 0;
800     }
801
802   /* Check signature */
803
804   /* Create packet + key cache */
805   /* First the OLSR packet + signature message - digest */
806   memcpy(checksum_cache, msg, sizeof(struct r_respmsg) - SIGNATURE_SIZE);
807   /* Then the key */
808   memcpy(&checksum_cache[sizeof(struct r_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
809   
810   /* Create the SHA1 hash */
811   SHA1(checksum_cache, 
812        (sizeof(struct r_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
813        sha1_hash);
814   
815   if(memcmp(sha1_hash, &msg->signature, 20) != 0)
816     {
817       olsr_printf(1, "[ENC]Signature missmatch in response-response!\n");
818       return 0;
819     }
820
821   olsr_printf(3, "[ENC]Signature verified\n");
822
823
824   /* Now to check the digest from the emitted challenge */
825   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
826     {
827       olsr_printf(1, "[ENC]Received response-response from non-registered node %s!\n",
828                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
829       return 0;
830     }
831
832   /* Generate the digest */
833   olsr_printf(3, "[ENC]Entry-challenge 0x%x\n", entry->challenge);
834
835   /* First the challenge received */
836   memcpy(checksum_cache, &entry->challenge, 4);
837   /* Then the local IP */
838   memcpy(&checksum_cache[sizeof(olsr_u32_t)], &msg->originator, ipsize);
839
840   /* Create the SHA1 hash */
841   SHA1(checksum_cache, 
842        sizeof(olsr_u32_t) + ipsize, 
843        sha1_hash);
844
845
846   if(memcmp(msg->res_sig, sha1_hash, 20) != 0)
847     {
848       olsr_printf(1, "[ENC]Error in response signature from %s!\n",
849                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
850       
851       return 0;
852     }
853
854   olsr_printf(3, "[ENC]Challenge-response signature ok\n");
855
856   /* Update entry! */
857
858
859   entry->challenge = 0;
860   entry->validated = 1;
861   entry->diff = now->tv_sec - msg->timestamp;
862
863   /* update validtime - validated entry */
864   olsr_get_timestamp((olsr_u32_t) TIMESTAMP_HOLD_TIME*1000, &entry->valtime);
865
866   olsr_printf(1, "[ENC]%s registered with diff %d!\n",
867               olsr_ip_to_string((union olsr_ip_addr *)&msg->originator),
868               entry->diff);
869
870   return 1;
871 }
872
873
874 int
875 parse_challenge(char *in_msg)
876 {
877   struct challengemsg *msg;
878   char sha1_hash[20];
879   struct stamp *entry;
880   olsr_u32_t hash;
881
882   msg = (struct challengemsg *)in_msg;
883
884   olsr_printf(1, "[ENC]Challenge message received\n");
885   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
886
887   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
888     {
889       olsr_printf(1, "[ENC]Not for us...\n");
890       return 0;
891     }
892
893   /* Create entry if not registered */
894   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
895     {
896       entry = malloc(sizeof(struct stamp));
897       memcpy(&entry->addr, &msg->originator, ipsize);
898
899       hash = olsr_hashing((union olsr_ip_addr *)&msg->originator);
900   
901       /* Queue */
902       timestamps[hash].next->prev = entry;
903       entry->next = timestamps[hash].next;
904       timestamps[hash].next = entry;
905       entry->prev = &timestamps[hash];
906     }
907   else
908     {
909       /* Check configuration timeout */
910       if(!TIMED_OUT(&entry->conftime))
911         {
912           /* If registered - do not accept! */
913           olsr_printf(1, "[ENC]Challenge from registered node...dropping!\n");
914           return 0;
915         }
916       else
917         {
918           olsr_printf(1, "[ENC]Challenge from registered node...accepted!\n");
919         }
920     }
921
922   olsr_printf(3, "[ENC]Challenge: 0x%x\n", ntohl(msg->challenge));
923
924   /* Check signature */
925
926   /* Create packet + key cache */
927   /* First the OLSR packet + signature message - digest */
928   memcpy(checksum_cache, msg, sizeof(struct challengemsg) - SIGNATURE_SIZE);
929   /* Then the key */
930   memcpy(&checksum_cache[sizeof(struct challengemsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
931   
932   /* Create the SHA1 hash */
933   SHA1(checksum_cache, 
934        (sizeof(struct challengemsg) - SIGNATURE_SIZE) + KEYLENGTH, 
935        sha1_hash);
936   
937   if(memcmp(sha1_hash, &msg->signature, 20) != 0)
938     {
939       olsr_printf(1, "[ENC]Signature missmatch in challenge!\n");
940       return 0;
941     }
942
943   olsr_printf(3, "[ENC]Signature verified\n");
944
945   
946   entry->diff = 0;
947   entry->validated = 0;
948
949   /* update validtime - not validated */
950   olsr_get_timestamp((olsr_u32_t) EXCHANGE_HOLD_TIME*1000, &entry->conftime);
951
952
953   /* Build and send response */
954
955   send_cres((union olsr_ip_addr *)&msg->originator, 
956             (union olsr_ip_addr *)&msg->destination, 
957             ntohl(msg->challenge),
958             entry);
959
960   return 1;
961 }
962
963
964
965
966
967 /**
968  * Build and transmit a challenge response
969  * message.
970  *
971  */
972 int
973 send_cres(union olsr_ip_addr *to, union olsr_ip_addr *from, olsr_u32_t chal_in, struct stamp *entry)
974 {
975   struct c_respmsg crmsg;
976   olsr_u32_t challenge;
977
978   olsr_printf(1, "[ENC]Building CRESPONSE message\n");
979
980   challenge = rand() << 16;
981   challenge |= rand();
982
983   entry->challenge = challenge;
984
985   olsr_printf(3, "[ENC]Challenge-response: 0x%x\n", challenge);
986
987   /* Fill challengemessage */
988   crmsg.olsr_msgtype = TYPE_CRESPONSE;
989   crmsg.olsr_vtime = 0;
990   crmsg.olsr_msgsize = htons(sizeof(struct c_respmsg));
991   memcpy(&crmsg.originator, main_addr, ipsize);
992   crmsg.ttl = 1;
993   crmsg.hopcnt = 0;
994   crmsg.seqno = htons(get_msg_seqno());
995
996   /* set timestamp */
997   crmsg.timestamp = now->tv_sec;
998   olsr_printf(3, "[ENC]Timestamp %d\n", crmsg.timestamp);
999
1000   /* Fill subheader */
1001   memcpy(&crmsg.destination, to, ipsize);
1002   crmsg.challenge = htonl(challenge);
1003
1004   /* Create digest of received challenge + IP */
1005
1006   /* Create packet + key cache */
1007   /* First the challenge received */
1008   memcpy(checksum_cache, &chal_in, 4);
1009   /* Then the local IP */
1010   memcpy(&checksum_cache[sizeof(olsr_u32_t)], from, ipsize);
1011
1012   /* Create the SHA1 hash */
1013   SHA1(checksum_cache, 
1014        sizeof(olsr_u32_t) + ipsize, 
1015        crmsg.res_sig);
1016
1017
1018   /* Now create the digest of the message and the key */
1019
1020   /* Create packet + key cache */
1021   /* First the OLSR packet + signature message - digest */
1022   memcpy(checksum_cache, &crmsg, sizeof(struct c_respmsg) - SIGNATURE_SIZE);
1023   /* Then the key */
1024   memcpy(&checksum_cache[sizeof(struct c_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
1025
1026   /* Create the SHA1 hash */
1027   SHA1(checksum_cache, 
1028        (sizeof(struct c_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
1029        crmsg.signature);
1030
1031   olsr_printf(3, "[ENC]Sending challenge response to %s challenge 0x%x\n", 
1032               olsr_ip_to_string(to),
1033               challenge);
1034
1035   /* Add to buffer */
1036   net_outbuffer_push(olsr_in_if, &crmsg, sizeof(struct c_respmsg));
1037   /* Send the request */
1038   net_output(olsr_in_if);
1039
1040   return 1;
1041 }
1042
1043
1044
1045
1046
1047
1048 /**
1049  * Build and transmit a response response
1050  * message.
1051  *
1052  */
1053 int
1054 send_rres(union olsr_ip_addr *to, union olsr_ip_addr *from, olsr_u32_t chal_in)
1055 {
1056   struct r_respmsg rrmsg;
1057
1058   olsr_printf(1, "[ENC]Building RRESPONSE message\n");
1059
1060
1061   /* Fill challengemessage */
1062   rrmsg.olsr_msgtype = TYPE_RRESPONSE;
1063   rrmsg.olsr_vtime = 0;
1064   rrmsg.olsr_msgsize = htons(sizeof(struct r_respmsg));
1065   memcpy(&rrmsg.originator, main_addr, ipsize);
1066   rrmsg.ttl = 1;
1067   rrmsg.hopcnt = 0;
1068   rrmsg.seqno = htons(get_msg_seqno());
1069
1070   /* set timestamp */
1071   rrmsg.timestamp = now->tv_sec;
1072   olsr_printf(3, "[ENC]Timestamp %d\n", rrmsg.timestamp);
1073
1074   /* Fill subheader */
1075   memcpy(&rrmsg.destination, to, ipsize);
1076
1077   /* Create digest of received challenge + IP */
1078
1079   /* Create packet + key cache */
1080   /* First the challenge received */
1081   memcpy(checksum_cache, &chal_in, 4);
1082   /* Then the local IP */
1083   memcpy(&checksum_cache[sizeof(olsr_u32_t)], from, ipsize);
1084
1085   /* Create the SHA1 hash */
1086   SHA1(checksum_cache, 
1087        sizeof(olsr_u32_t) + ipsize, 
1088        rrmsg.res_sig);
1089
1090
1091   /* Now create the digest of the message and the key */
1092
1093   /* Create packet + key cache */
1094   /* First the OLSR packet + signature message - digest */
1095   memcpy(checksum_cache, &rrmsg, sizeof(struct r_respmsg) - SIGNATURE_SIZE);
1096   /* Then the key */
1097   memcpy(&checksum_cache[sizeof(struct r_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
1098
1099   /* Create the SHA1 hash */
1100   SHA1(checksum_cache, 
1101        (sizeof(struct r_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
1102        rrmsg.signature);
1103
1104   olsr_printf(3, "[ENC]Sending response response to %s\n", 
1105               olsr_ip_to_string(to));
1106
1107   /* add to buffer */
1108   net_outbuffer_push(olsr_in_if, &rrmsg, sizeof(struct r_respmsg));
1109
1110   /* Send the request */
1111   net_output(olsr_in_if);
1112
1113   return 1;
1114 }
1115
1116
1117
1118 struct stamp *
1119 lookup_timestamp_entry(union olsr_ip_addr *adr)
1120 {
1121   olsr_u32_t hash;
1122   struct stamp *entry;
1123
1124   hash = olsr_hashing(adr);
1125
1126   for(entry = timestamps[hash].next;
1127       entry != &timestamps[hash];
1128       entry = entry->next)
1129     {
1130       if(memcmp(&entry->addr, adr, ipsize) == 0)
1131         {
1132           olsr_printf(3, "[ENC]Match for %s\n", olsr_ip_to_string(adr));
1133           return entry;
1134         }
1135     }
1136
1137   olsr_printf(1, "[ENC]No match for %s\n", olsr_ip_to_string(adr));
1138
1139   return NULL;
1140 }
1141
1142
1143
1144 /**
1145  *Find timed out entries and delete them
1146  *
1147  *@return nada
1148  */
1149 void
1150 timeout_timestamps()
1151 {
1152   struct stamp *tmp_list;
1153   struct stamp *entry_to_delete;
1154   int index;
1155
1156
1157   for(index=0;index<HASHSIZE;index++)
1158     {
1159       tmp_list = timestamps[index].next;
1160       /*Traverse MID list*/
1161       while(tmp_list != &timestamps[index])
1162         {
1163           /*Check if the entry is timed out*/
1164           if((TIMED_OUT(&tmp_list->valtime)) && (TIMED_OUT(&tmp_list->conftime)))
1165             {
1166               entry_to_delete = tmp_list;
1167               tmp_list = tmp_list->next;
1168
1169               olsr_printf(1, "[ENC]timestamp info for %s timed out.. deleting it\n", 
1170                           olsr_ip_to_string(&entry_to_delete->addr));
1171
1172               /*Delete it*/
1173               entry_to_delete->next->prev = entry_to_delete->prev;
1174               entry_to_delete->prev->next = entry_to_delete->next;
1175
1176               free(entry_to_delete);
1177             }
1178           else
1179               tmp_list = tmp_list->next;
1180         }
1181     }
1182
1183   return;
1184 }
1185
1186
1187
1188 int
1189 read_key_from_file(char *file)
1190 {
1191   FILE *kf;
1192   size_t keylen;
1193
1194   keylen = 16;
1195   kf = fopen(file, "r");
1196
1197   if(kf == NULL)
1198     {
1199       olsr_printf(1, "[ENC]Could not open keyfile %s!\nError: %s\n", file, strerror(errno));
1200       return -1;
1201     }
1202
1203   if(fread(aes_key, 1, keylen, kf) != keylen)
1204     {
1205       olsr_printf(1, "[ENC]Could not read key from keyfile %s!\nError: %s\n", file, strerror(errno));
1206       fclose(kf);
1207       return -1;
1208     }
1209
1210
1211   fclose(kf);
1212   return 0;
1213 }
1214
1215
1216 /*************************************************************
1217  *                 TOOLS DERIVED FROM OLSRD                  *
1218  *************************************************************/
1219
1220
1221 /**
1222  *Hashing function. Creates a key based on
1223  *an 32-bit address.
1224  *@param address the address to hash
1225  *@return the hash(a value in the 0-31 range)
1226  */
1227 olsr_u32_t
1228 olsr_hashing(union olsr_ip_addr *address)
1229 {
1230   olsr_u32_t hash;
1231   char *tmp;
1232
1233   if(ipversion == AF_INET)
1234     /* IPv4 */  
1235     hash = (ntohl(address->v4));
1236   else
1237     {
1238       /* IPv6 */
1239       tmp = (char *) &address->v6;
1240       hash = (ntohl(*tmp));
1241     }
1242
1243   //hash &= 0x7fffffff; 
1244   hash &= HASHMASK;
1245
1246   return hash;
1247 }
1248
1249
1250
1251 /**
1252  *Checks if a timer has times out. That means
1253  *if it is smaller than present time.
1254  *@param timer the timeval struct to evaluate
1255  *@return positive if the timer has not timed out,
1256  *0 if it matches with present time and negative
1257  *if it is timed out.
1258  */
1259 int
1260 olsr_timed_out(struct timeval *timer)
1261 {
1262   return(timercmp(timer, now, <));
1263 }
1264
1265
1266
1267 /**
1268  *Initiates a "timer", wich is a timeval structure,
1269  *with the value given in time_value.
1270  *@param time_value the value to initialize the timer with
1271  *@param hold_timer the timer itself
1272  *@return nada
1273  */
1274 void
1275 olsr_init_timer(olsr_u32_t time_value, struct timeval *hold_timer)
1276
1277   olsr_u16_t  time_value_sec;
1278   olsr_u16_t  time_value_msec;
1279
1280   time_value_sec = time_value/1000;
1281   time_value_msec = time_value-(time_value_sec*1000);
1282
1283   hold_timer->tv_sec = time_value_sec;
1284   hold_timer->tv_usec = time_value_msec*1000;   
1285 }
1286
1287
1288
1289
1290
1291 /**
1292  *Generaties a timestamp a certain number of milliseconds
1293  *into the future.
1294  *
1295  *@param time_value how many milliseconds from now
1296  *@param hold_timer the timer itself
1297  *@return nada
1298  */
1299 void
1300 olsr_get_timestamp(olsr_u32_t delay, struct timeval *hold_timer)
1301
1302   olsr_u16_t  time_value_sec;
1303   olsr_u16_t  time_value_msec;
1304
1305   time_value_sec = delay/1000;
1306   time_value_msec= delay - (delay*1000);
1307
1308   hold_timer->tv_sec = now->tv_sec + time_value_sec;
1309   hold_timer->tv_usec = now->tv_usec + (time_value_msec*1000);   
1310 }
1311
1312
1313 /**
1314  *Converts a olsr_ip_addr to a string
1315  *Goes for both IPv4 and IPv6
1316  *
1317  *NON REENTRANT! If you need to use this
1318  *function twice in e.g. the same printf
1319  *it will not work.
1320  *You must use it in different calls e.g.
1321  *two different printfs
1322  *
1323  *@param the IP to convert
1324  *@return a pointer to a static string buffer
1325  *representing the address in "dots and numbers"
1326  *
1327  */
1328 char *
1329 olsr_ip_to_string(union olsr_ip_addr *addr)
1330 {
1331
1332   char *ret;
1333   struct in_addr in;
1334   
1335   if(ipversion == AF_INET)
1336     {
1337       in.s_addr=addr->v4;
1338       ret = inet_ntoa(in);
1339     }
1340   else
1341     {
1342       /* IPv6 */
1343       ret = (char *)inet_ntop(AF_INET6, &addr->v6, ipv6_buf, sizeof(ipv6_buf));
1344     }
1345
1346   return ret;
1347 }
1348
1349