* applied patches from the most recent FreiFunkFirmware (and fixed compile errors...
[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.21 2007/01/31 12:36:49 bernd67 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(const olsr_u8_t *data, const olsr_u16_t data_len, olsr_u8_t *hashbuf)
75 {
76   MD5_CTX context;
77
78   MD5Init(&context);
79   MD5Update(&context, data, data_len);
80   MD5Final(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, olsr_cnf->ipsize);
277         }
278       else
279         {
280           /* IPv6 sender address */
281           memcpy(&from_addr, &((struct sockaddr_in6 *)&from)->sin6_addr, olsr_cnf->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(olsr_u8_t *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, &olsr_cnf->main_addr, olsr_cnf->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   {
435   olsr_u8_t checksum_cache[512 + KEYLENGTH];
436   /* Create packet + key cache */
437   /* First the OLSR packet + signature message - digest */
438   memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
439   /* Then the key */
440   memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);
441   
442   /* Create the hash */
443   CHECKSUM(checksum_cache, (*size - SIGNATURE_SIZE) + KEYLENGTH, &pck[*size - SIGNATURE_SIZE]);
444   } 
445
446 #ifdef DEBUG
447   olsr_printf(1, "Signature message:\n");
448
449   j = 0;
450   sigmsg = (char *)msg;
451
452   for(i = 0; i < sizeof(struct s_olsrmsg); i++)
453     {
454       olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
455       j++;
456       if(j == 4)
457         {
458           olsr_printf(1, "\n");
459           j = 0;
460         }
461     }
462 #endif
463
464   olsr_printf(3, "[ENC] Message signed\n");
465
466   return 1;
467 }
468
469
470
471 int
472 validate_packet(char *pck, int *size)
473 {
474   int packetsize;
475   olsr_u8_t sha1_hash[SIGNATURE_SIZE];
476   struct s_olsrmsg *sig;
477   time_t rec_time;
478
479 #ifdef DEBUG
480   int i, j;
481   char *sigmsg;
482 #endif
483
484   /* Find size - signature message */
485   packetsize = *size - sizeof(struct s_olsrmsg);
486
487   if(packetsize < 4)
488     return 0;
489
490   sig = (struct s_olsrmsg *)&pck[packetsize];
491
492   //olsr_printf(1, "Size: %d\n", packetsize);
493
494 #ifdef DEBUG
495   olsr_printf(1, "Input message:\n");
496   
497   j = 0;
498   sigmsg = (char *)sig;
499
500   for(i = 0; i < sizeof(struct s_olsrmsg); i++)
501     {
502       olsr_printf(1, "  %3i", (u_char) sigmsg[i]);
503       j++;
504       if(j == 4)
505         {
506           olsr_printf(1, "\n");
507           j = 0;
508         }
509     }
510 #endif
511
512   /* Sanity check first */
513   if((sig->olsr_msgtype != MESSAGE_TYPE) || 
514      (sig->olsr_vtime) ||
515      (sig->olsr_msgsize != ntohs(sizeof(struct s_olsrmsg))) ||
516      (sig->ttl != 1) ||
517      (sig->hopcnt != 0))
518     {
519       olsr_printf(1, "[ENC]Packet not sane!\n");
520       return 0;
521     }
522
523   /* Check scheme and type */
524   switch(sig->sig.type)
525     {
526     case(ONE_CHECKSUM):
527       switch(sig->sig.algorithm)
528         {
529         case(SCHEME):
530           goto one_checksum_SHA; /* Ahhh... fix this */
531           break;
532           
533         }
534       break;
535
536     default:
537       olsr_printf(1, "[ENC]Unsupported sceme: %d enc: %d!\n", sig->sig.type, sig->sig.algorithm);
538       return 0;
539       break;
540
541     }
542   //olsr_printf(1, "Packet sane...\n");
543
544  one_checksum_SHA:
545  
546  {
547   olsr_u8_t checksum_cache[512 + KEYLENGTH];
548   /* Create packet + key cache */
549   /* First the OLSR packet + signature message - digest */
550   memcpy(checksum_cache, pck, *size - SIGNATURE_SIZE);
551   /* Then the key */
552   memcpy(&checksum_cache[*size - SIGNATURE_SIZE], aes_key, KEYLENGTH);
553
554   /* generate SHA-1 */
555   CHECKSUM(checksum_cache, *size - SIGNATURE_SIZE + KEYLENGTH, sha1_hash);
556  }
557
558 #ifdef DEBUG
559   olsr_printf(1, "Recevied hash:\n");
560   
561   sigmsg = (char *)sig->sig.signature;
562
563   for(i = 0; i < SIGNATURE_SIZE; i++)
564     {
565       olsr_printf(1, " %3i", (u_char) sigmsg[i]);
566     }
567   olsr_printf(1, "\n");
568
569   olsr_printf(1, "Calculated hash:\n");
570   
571   sigmsg = sha1_hash;
572
573   for(i = 0; i < SIGNATURE_SIZE; i++)
574     {
575       olsr_printf(1, " %3i", (u_char) sigmsg[i]);
576     }
577   olsr_printf(1, "\n");
578 #endif
579
580   if(memcmp(sha1_hash, sig->sig.signature, SIGNATURE_SIZE) != 0)
581     {
582       olsr_printf(1, "[ENC]Signature missmatch\n");
583       return 0;
584     }
585
586   /* Check timestamp */
587   rec_time = ntohl(sig->sig.timestamp);
588
589   if(!check_timestamp((union olsr_ip_addr *)&sig->originator, rec_time))
590     {
591       olsr_printf(1, "[ENC]Timestamp missmatch in packet from %s!\n",
592                   olsr_ip_to_string((union olsr_ip_addr *)&sig->originator));
593       return 0;
594     }
595
596   olsr_printf(1, "[ENC]Received timestamp %d diff: %d\n", rec_time, now.tv_sec - rec_time);
597
598   /* Remove signature message */
599   *size = packetsize;
600   return 1;
601 }
602
603
604 int
605 check_timestamp(union olsr_ip_addr *originator, time_t tstamp)
606 {
607   struct stamp *entry;
608   int diff;
609
610   entry = lookup_timestamp_entry(originator);
611
612   if(!entry)
613     {
614       /* Initiate timestamp negotiation */
615
616       send_challenge(originator);
617
618       return 0;
619     }
620
621   if(!entry->validated)
622     {
623       olsr_printf(1, "[ENC]Message from non-validated host!\n");
624       return 0;
625     }
626
627   diff = entry->diff - (now.tv_sec - tstamp);
628
629   olsr_printf(3, "[ENC]Timestamp slack: %d\n", diff);
630
631   if((diff > UPPER_DIFF) || (diff < LOWER_DIFF))
632     {
633       olsr_printf(1, "[ENC]Timestamp scew detected!!\n");
634       return 0;
635     }
636
637   /* ok - update diff */
638   entry->diff = ((now.tv_sec - tstamp) + entry->diff) ?
639     ((now.tv_sec - tstamp) + entry->diff) / 2 : 0;
640
641   olsr_printf(3, "[ENC]Diff set to : %d\n", entry->diff);
642
643   /* update validtime */
644
645   entry->valtime = GET_TIMESTAMP(TIMESTAMP_HOLD_TIME * 1000);
646
647   return 1;
648 }
649
650
651 /**
652  * Create and send a timestamp
653  * challenge message to new_host
654  *
655  * The host is registered in the timestamps
656  * repository with valid=0
657  */
658
659 int
660 send_challenge(union olsr_ip_addr *new_host)
661 {
662   struct challengemsg cmsg;
663   struct stamp *entry;
664   olsr_u32_t challenge, hash;
665
666   olsr_printf(1, "[ENC]Building CHALLENGE message\n");
667
668   /* Set the size including OLSR packet size */
669
670
671   challenge = rand() << 16;
672   challenge |= rand();
673
674   /* Fill challengemessage */
675   cmsg.olsr_msgtype = TYPE_CHALLENGE;
676   cmsg.olsr_vtime = 0;
677   cmsg.olsr_msgsize = htons(sizeof(struct challengemsg));
678   memcpy(&cmsg.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
679   cmsg.ttl = 1;
680   cmsg.hopcnt = 0;
681   cmsg.seqno = htons(get_msg_seqno());
682
683   /* Fill subheader */
684   memcpy(&cmsg.destination, new_host, olsr_cnf->ipsize);
685   cmsg.challenge = htonl(challenge);
686
687   olsr_printf(3, "[ENC]Size: %d\n", sizeof(struct challengemsg));
688
689   {
690   olsr_u8_t checksum_cache[512 + KEYLENGTH];
691   /* Create packet + key cache */
692   /* First the OLSR packet + signature message - digest */
693   memcpy(checksum_cache, &cmsg, sizeof(struct challengemsg) - SIGNATURE_SIZE);
694   /* Then the key */
695   memcpy(&checksum_cache[sizeof(struct challengemsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
696
697   /* Create the hash */
698   CHECKSUM(checksum_cache, 
699            (sizeof(struct challengemsg) - SIGNATURE_SIZE) + KEYLENGTH, 
700            cmsg.signature);
701   }
702   olsr_printf(3, "[ENC]Sending timestamp request to %s challenge 0x%x\n", 
703               olsr_ip_to_string(new_host),
704               challenge);
705
706   /* Add to buffer */
707   net_outbuffer_push(olsr_in_if, (olsr_u8_t *)&cmsg, sizeof(struct challengemsg));
708
709   /* Send the request */
710   net_output(olsr_in_if);
711
712   /* Create new entry */
713   entry = malloc(sizeof(struct stamp));
714   
715   entry->diff = 0;
716   entry->validated = 0;
717   entry->challenge = challenge;
718
719   memcpy(&entry->addr, new_host, olsr_cnf->ipsize);
720
721   /* update validtime - not validated */
722   entry->conftime = GET_TIMESTAMP(EXCHANGE_HOLD_TIME * 1000);
723
724   hash = olsr_hashing(new_host);
725   
726   /* Queue */
727   timestamps[hash].next->prev = entry;
728   entry->next = timestamps[hash].next;
729   timestamps[hash].next = entry;
730   entry->prev = &timestamps[hash];
731
732
733   return 1;
734
735 }
736
737 int
738 parse_cres(char *in_msg)
739 {
740   struct c_respmsg *msg;
741   olsr_u8_t sha1_hash[SIGNATURE_SIZE];
742   struct stamp *entry;
743
744   msg = (struct c_respmsg *)in_msg;
745
746   olsr_printf(1, "[ENC]Challenge-response message received\n");
747   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
748
749   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
750     {
751       olsr_printf(3, "[ENC]Not for us...\n");
752       return 0;
753     }
754
755   olsr_printf(3, "[ENC]Challenge: 0x%x\n", ntohl(msg->challenge));
756
757   /* Check signature */
758
759   {
760   olsr_u8_t checksum_cache[512 + KEYLENGTH];
761   /* Create packet + key cache */
762   /* First the OLSR packet + signature message - digest */
763   memcpy(checksum_cache, msg, sizeof(struct c_respmsg) - SIGNATURE_SIZE);
764   /* Then the key */
765   memcpy(&checksum_cache[sizeof(struct c_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
766   
767   /* Create the hash */
768   CHECKSUM(checksum_cache, 
769            (sizeof(struct c_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
770            sha1_hash);
771   }
772   
773   if(memcmp(sha1_hash, &msg->signature, SIGNATURE_SIZE) != 0)
774     {
775       olsr_printf(1, "[ENC]Signature missmatch in challenge-response!\n");
776       return 0;
777     }
778
779   olsr_printf(3, "[ENC]Signature verified\n");
780
781
782   /* Now to check the digest from the emitted challenge */
783   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
784     {
785       olsr_printf(1, "[ENC]Received challenge-response from non-registered node %s!\n",
786                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
787       return 0;
788     }
789
790   /* Generate the digest */
791   olsr_printf(3, "[ENC]Entry-challenge 0x%x\n", entry->challenge);
792
793   {
794   olsr_u8_t checksum_cache[512 + KEYLENGTH];
795   /* First the challenge received */
796   memcpy(checksum_cache, &entry->challenge, 4);
797   /* Then the local IP */
798   memcpy(&checksum_cache[sizeof(olsr_u32_t)], &msg->originator, olsr_cnf->ipsize);
799
800   /* Create the hash */
801   CHECKSUM(checksum_cache, 
802            sizeof(olsr_u32_t) + olsr_cnf->ipsize, 
803            sha1_hash);
804   }
805
806   if(memcmp(msg->res_sig, sha1_hash, SIGNATURE_SIZE) != 0)
807     {
808       olsr_printf(1, "[ENC]Error in challenge signature from %s!\n",
809                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
810       
811       return 0;
812     }
813
814   olsr_printf(3, "[ENC]Challenge-response signature ok\n");
815
816   /* Update entry! */
817
818
819   entry->challenge = 0;
820   entry->validated = 1;
821   entry->diff = now.tv_sec - msg->timestamp;
822
823   /* update validtime - validated entry */
824   entry->valtime = GET_TIMESTAMP(TIMESTAMP_HOLD_TIME * 1000);
825
826   olsr_printf(1, "[ENC]%s registered with diff %d!\n",
827               olsr_ip_to_string((union olsr_ip_addr *)&msg->originator),
828               entry->diff);
829
830   /* Send response-response */
831   send_rres((union olsr_ip_addr *)&msg->originator, 
832             (union olsr_ip_addr *)&msg->destination, 
833             ntohl(msg->challenge));
834
835   return 1;
836 }
837
838
839 int
840 parse_rres(char *in_msg)
841 {
842   struct r_respmsg *msg;
843   olsr_u8_t sha1_hash[SIGNATURE_SIZE];
844   struct stamp *entry;
845
846   msg = (struct r_respmsg *)in_msg;
847
848   olsr_printf(1, "[ENC]Response-response message received\n");
849   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
850
851   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
852     {
853       olsr_printf(1, "[ENC]Not for us...\n");
854       return 0;
855     }
856
857   /* Check signature */
858
859   {
860   olsr_u8_t checksum_cache[512 + KEYLENGTH];
861   /* Create packet + key cache */
862   /* First the OLSR packet + signature message - digest */
863   memcpy(checksum_cache, msg, sizeof(struct r_respmsg) - SIGNATURE_SIZE);
864   /* Then the key */
865   memcpy(&checksum_cache[sizeof(struct r_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
866   
867   /* Create the hash */
868   CHECKSUM(checksum_cache, 
869            (sizeof(struct r_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
870            sha1_hash);
871   }
872   
873   if(memcmp(sha1_hash, &msg->signature, SIGNATURE_SIZE) != 0)
874     {
875       olsr_printf(1, "[ENC]Signature missmatch in response-response!\n");
876       return 0;
877     }
878
879   olsr_printf(3, "[ENC]Signature verified\n");
880
881
882   /* Now to check the digest from the emitted challenge */
883   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
884     {
885       olsr_printf(1, "[ENC]Received response-response from non-registered node %s!\n",
886                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
887       return 0;
888     }
889
890   /* Generate the digest */
891   olsr_printf(3, "[ENC]Entry-challenge 0x%x\n", entry->challenge);
892
893   {
894   olsr_u8_t checksum_cache[512 + KEYLENGTH];
895   /* First the challenge received */
896   memcpy(checksum_cache, &entry->challenge, 4);
897   /* Then the local IP */
898   memcpy(&checksum_cache[sizeof(olsr_u32_t)], &msg->originator, olsr_cnf->ipsize);
899
900   /* Create the hash */
901   CHECKSUM(checksum_cache, 
902            sizeof(olsr_u32_t) + olsr_cnf->ipsize, 
903            sha1_hash);
904   }
905
906   if(memcmp(msg->res_sig, sha1_hash, SIGNATURE_SIZE) != 0)
907     {
908       olsr_printf(1, "[ENC]Error in response signature from %s!\n",
909                   olsr_ip_to_string((union olsr_ip_addr *)&msg->originator));
910       
911       return 0;
912     }
913
914   olsr_printf(3, "[ENC]Challenge-response signature ok\n");
915
916   /* Update entry! */
917
918
919   entry->challenge = 0;
920   entry->validated = 1;
921   entry->diff = now.tv_sec - msg->timestamp;
922
923   /* update validtime - validated entry */
924   entry->valtime = GET_TIMESTAMP(TIMESTAMP_HOLD_TIME * 1000);
925
926   olsr_printf(1, "[ENC]%s registered with diff %d!\n",
927               olsr_ip_to_string((union olsr_ip_addr *)&msg->originator),
928               entry->diff);
929
930   return 1;
931 }
932
933
934 int
935 parse_challenge(char *in_msg)
936 {
937   struct challengemsg *msg;
938   olsr_u8_t sha1_hash[SIGNATURE_SIZE];
939   struct stamp *entry;
940   olsr_u32_t hash;
941
942   msg = (struct challengemsg *)in_msg;
943
944   olsr_printf(1, "[ENC]Challenge message received\n");
945   olsr_printf(3, "[ENC]To: %s\n", olsr_ip_to_string((union olsr_ip_addr *)&msg->destination));
946
947   if(if_ifwithaddr((union olsr_ip_addr *)&msg->destination) == NULL)
948     {
949       olsr_printf(1, "[ENC]Not for us...\n");
950       return 0;
951     }
952
953   /* Create entry if not registered */
954   if((entry = lookup_timestamp_entry((union olsr_ip_addr *)&msg->originator)) == NULL)
955     {
956       entry = malloc(sizeof(struct stamp));
957       memcpy(&entry->addr, &msg->originator, olsr_cnf->ipsize);
958
959       hash = olsr_hashing((union olsr_ip_addr *)&msg->originator);
960   
961       /* Queue */
962       timestamps[hash].next->prev = entry;
963       entry->next = timestamps[hash].next;
964       timestamps[hash].next = entry;
965       entry->prev = &timestamps[hash];
966     }
967   else
968     {
969       /* Check configuration timeout */
970       if(!TIMED_OUT(entry->conftime))
971         {
972           /* If registered - do not accept! */
973           olsr_printf(1, "[ENC]Challenge from registered node...dropping!\n");
974           return 0;
975         }
976       else
977         {
978           olsr_printf(1, "[ENC]Challenge from registered node...accepted!\n");
979         }
980     }
981
982   olsr_printf(3, "[ENC]Challenge: 0x%x\n", ntohl(msg->challenge));
983
984   /* Check signature */
985
986   {
987   olsr_u8_t checksum_cache[512 + KEYLENGTH];
988   /* Create packet + key cache */
989   /* First the OLSR packet + signature message - digest */
990   memcpy(checksum_cache, msg, sizeof(struct challengemsg) - SIGNATURE_SIZE);
991   /* Then the key */
992   memcpy(&checksum_cache[sizeof(struct challengemsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
993   
994   /* Create the hash */
995   CHECKSUM(checksum_cache, 
996            (sizeof(struct challengemsg) - SIGNATURE_SIZE) + KEYLENGTH, 
997            sha1_hash);
998   }  
999   if(memcmp(sha1_hash, &msg->signature, SIGNATURE_SIZE) != 0)
1000     {
1001       olsr_printf(1, "[ENC]Signature missmatch in challenge!\n");
1002       return 0;
1003     }
1004
1005   olsr_printf(3, "[ENC]Signature verified\n");
1006
1007   
1008   entry->diff = 0;
1009   entry->validated = 0;
1010
1011   /* update validtime - not validated */
1012   entry->conftime = GET_TIMESTAMP(EXCHANGE_HOLD_TIME * 1000);
1013
1014   /* Build and send response */
1015
1016   send_cres((union olsr_ip_addr *)&msg->originator, 
1017             (union olsr_ip_addr *)&msg->destination, 
1018             ntohl(msg->challenge),
1019             entry);
1020
1021   return 1;
1022 }
1023
1024
1025
1026
1027
1028 /**
1029  * Build and transmit a challenge response
1030  * message.
1031  *
1032  */
1033 int
1034 send_cres(union olsr_ip_addr *to, union olsr_ip_addr *from, olsr_u32_t chal_in, struct stamp *entry)
1035 {
1036   struct c_respmsg crmsg;
1037   olsr_u32_t challenge;
1038
1039   olsr_printf(1, "[ENC]Building CRESPONSE message\n");
1040
1041   challenge = rand() << 16;
1042   challenge |= rand();
1043
1044   entry->challenge = challenge;
1045
1046   olsr_printf(3, "[ENC]Challenge-response: 0x%x\n", challenge);
1047
1048   /* Fill challengemessage */
1049   crmsg.olsr_msgtype = TYPE_CRESPONSE;
1050   crmsg.olsr_vtime = 0;
1051   crmsg.olsr_msgsize = htons(sizeof(struct c_respmsg));
1052   memcpy(&crmsg.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
1053   crmsg.ttl = 1;
1054   crmsg.hopcnt = 0;
1055   crmsg.seqno = htons(get_msg_seqno());
1056
1057   /* set timestamp */
1058   crmsg.timestamp = now.tv_sec;
1059   olsr_printf(3, "[ENC]Timestamp %d\n", crmsg.timestamp);
1060
1061   /* Fill subheader */
1062   memcpy(&crmsg.destination, to, olsr_cnf->ipsize);
1063   crmsg.challenge = htonl(challenge);
1064
1065   /* Create digest of received challenge + IP */
1066
1067   {
1068   olsr_u8_t checksum_cache[512 + KEYLENGTH];
1069   /* Create packet + key cache */
1070   /* First the challenge received */
1071   memcpy(checksum_cache, &chal_in, 4);
1072   /* Then the local IP */
1073   memcpy(&checksum_cache[sizeof(olsr_u32_t)], from, olsr_cnf->ipsize);
1074
1075   /* Create the hash */
1076   CHECKSUM(checksum_cache, 
1077            sizeof(olsr_u32_t) + olsr_cnf->ipsize, 
1078            crmsg.res_sig);
1079   }
1080
1081   /* Now create the digest of the message and the key */
1082
1083   {
1084   olsr_u8_t checksum_cache[512 + KEYLENGTH];
1085   /* Create packet + key cache */
1086   /* First the OLSR packet + signature message - digest */
1087   memcpy(checksum_cache, &crmsg, sizeof(struct c_respmsg) - SIGNATURE_SIZE);
1088   /* Then the key */
1089   memcpy(&checksum_cache[sizeof(struct c_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
1090
1091   /* Create the hash */
1092   CHECKSUM(checksum_cache, 
1093            (sizeof(struct c_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
1094            crmsg.signature);
1095   }
1096
1097   olsr_printf(3, "[ENC]Sending challenge response to %s challenge 0x%x\n", 
1098               olsr_ip_to_string(to),
1099               challenge);
1100
1101   /* Add to buffer */
1102   net_outbuffer_push(olsr_in_if, (olsr_u8_t *)&crmsg, sizeof(struct c_respmsg));
1103   /* Send the request */
1104   net_output(olsr_in_if);
1105
1106   return 1;
1107 }
1108
1109
1110
1111
1112
1113
1114 /**
1115  * Build and transmit a response response
1116  * message.
1117  *
1118  */
1119 int
1120 send_rres(union olsr_ip_addr *to, union olsr_ip_addr *from, olsr_u32_t chal_in)
1121 {
1122   struct r_respmsg rrmsg;
1123
1124   olsr_printf(1, "[ENC]Building RRESPONSE message\n");
1125
1126
1127   /* Fill challengemessage */
1128   rrmsg.olsr_msgtype = TYPE_RRESPONSE;
1129   rrmsg.olsr_vtime = 0;
1130   rrmsg.olsr_msgsize = htons(sizeof(struct r_respmsg));
1131   memcpy(&rrmsg.originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
1132   rrmsg.ttl = 1;
1133   rrmsg.hopcnt = 0;
1134   rrmsg.seqno = htons(get_msg_seqno());
1135
1136   /* set timestamp */
1137   rrmsg.timestamp = now.tv_sec;
1138   olsr_printf(3, "[ENC]Timestamp %d\n", rrmsg.timestamp);
1139
1140   /* Fill subheader */
1141   memcpy(&rrmsg.destination, to, olsr_cnf->ipsize);
1142
1143   /* Create digest of received challenge + IP */
1144
1145   {
1146   olsr_u8_t checksum_cache[512 + KEYLENGTH];
1147   /* Create packet + key cache */
1148   /* First the challenge received */
1149   memcpy(checksum_cache, &chal_in, 4);
1150   /* Then the local IP */
1151   memcpy(&checksum_cache[sizeof(olsr_u32_t)], from, olsr_cnf->ipsize);
1152
1153   /* Create the hash */
1154   CHECKSUM(checksum_cache, 
1155            sizeof(olsr_u32_t) + olsr_cnf->ipsize, 
1156            rrmsg.res_sig);
1157   }
1158
1159   /* Now create the digest of the message and the key */
1160
1161   {
1162   olsr_u8_t checksum_cache[512 + KEYLENGTH];
1163   /* Create packet + key cache */
1164   /* First the OLSR packet + signature message - digest */
1165   memcpy(checksum_cache, &rrmsg, sizeof(struct r_respmsg) - SIGNATURE_SIZE);
1166   /* Then the key */
1167   memcpy(&checksum_cache[sizeof(struct r_respmsg) - SIGNATURE_SIZE], aes_key, KEYLENGTH);
1168
1169   /* Create the hash */
1170   CHECKSUM(checksum_cache, 
1171            (sizeof(struct r_respmsg) - SIGNATURE_SIZE) + KEYLENGTH, 
1172            rrmsg.signature);
1173   }
1174
1175   olsr_printf(3, "[ENC]Sending response response to %s\n", 
1176               olsr_ip_to_string(to));
1177
1178   /* add to buffer */
1179   net_outbuffer_push(olsr_in_if, (olsr_u8_t *)&rrmsg, sizeof(struct r_respmsg));
1180
1181   /* Send the request */
1182   net_output(olsr_in_if);
1183
1184   return 1;
1185 }
1186
1187
1188
1189 struct stamp *
1190 lookup_timestamp_entry(union olsr_ip_addr *adr)
1191 {
1192   olsr_u32_t hash;
1193   struct stamp *entry;
1194
1195   hash = olsr_hashing(adr);
1196
1197   for(entry = timestamps[hash].next;
1198       entry != &timestamps[hash];
1199       entry = entry->next)
1200     {
1201       if(memcmp(&entry->addr, adr, olsr_cnf->ipsize) == 0)
1202         {
1203           olsr_printf(3, "[ENC]Match for %s\n", olsr_ip_to_string(adr));
1204           return entry;
1205         }
1206     }
1207
1208   olsr_printf(1, "[ENC]No match for %s\n", olsr_ip_to_string(adr));
1209
1210   return NULL;
1211 }
1212
1213
1214
1215 /**
1216  *Find timed out entries and delete them
1217  *
1218  *@return nada
1219  */
1220 void
1221 timeout_timestamps(void* foo)
1222 {
1223   struct stamp *tmp_list;
1224   struct stamp *entry_to_delete;
1225   int index;
1226
1227
1228   for(index=0;index<HASHSIZE;index++)
1229     {
1230       tmp_list = timestamps[index].next;
1231       /*Traverse MID list*/
1232       while(tmp_list != &timestamps[index])
1233         {
1234           /*Check if the entry is timed out*/
1235           if((TIMED_OUT(tmp_list->valtime)) && (TIMED_OUT(tmp_list->conftime)))
1236             {
1237               entry_to_delete = tmp_list;
1238               tmp_list = tmp_list->next;
1239
1240               olsr_printf(1, "[ENC]timestamp info for %s timed out.. deleting it\n", 
1241                           olsr_ip_to_string(&entry_to_delete->addr));
1242
1243               /*Delete it*/
1244               entry_to_delete->next->prev = entry_to_delete->prev;
1245               entry_to_delete->prev->next = entry_to_delete->next;
1246
1247               free(entry_to_delete);
1248             }
1249           else
1250               tmp_list = tmp_list->next;
1251         }
1252     }
1253
1254   return;
1255 }
1256
1257
1258
1259 int
1260 read_key_from_file(char *file)
1261 {
1262   FILE *kf;
1263   size_t keylen;
1264
1265   keylen = 16;
1266   kf = fopen(file, "r");
1267
1268   olsr_printf(1, "[ENC]Reading key from file \"%s\"\n", file);
1269
1270   if(kf == NULL)
1271     {
1272       olsr_printf(1, "[ENC]Could not open keyfile %s!\nError: %s\n", file, strerror(errno));
1273       return -1;
1274     }
1275
1276   if(fread(aes_key, 1, keylen, kf) != keylen)
1277     {
1278       olsr_printf(1, "[ENC]Could not read key from keyfile %s!\nError: %s\n", file, strerror(errno));
1279       fclose(kf);
1280       return 0;
1281     }
1282
1283
1284   fclose(kf);
1285   return 1;
1286 }
1287