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