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