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