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