Rename "subsystems" directory to "base"
[oonf.git] / src / crypto / rfc5444_signature / rfc5444_signature.c
1
2 /*
3  * The olsr.org Optimized Link-State Routing daemon version 2 (olsrd2)
4  * Copyright (c) 2004-2015, the olsr.org team - see HISTORY file
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  *   notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  *   notice, this list of conditions and the following disclaimer in
15  *   the documentation and/or other materials provided with the
16  *   distribution.
17  * * Neither the name of olsr.org, olsrd nor the names of its
18  *   contributors may be used to endorse or promote products derived
19  *   from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * Visit http://www.olsr.org for more information.
35  *
36  * If you find this software useful feel free to make a donation
37  * to the project. For more information see the website or contact
38  * the copyright holders.
39  *
40  */
41
42 /**
43  * @file
44  */
45
46 #include <oonf/libcommon/avl.h>
47 #include <oonf/libcommon/avl_comp.h>
48 #include <oonf/oonf.h>
49 #include <oonf/libcore/oonf_subsystem.h>
50 #include <oonf/crypto/rfc7182_provider/rfc7182_provider.h>
51 #include <oonf/base/oonf_class.h>
52 #include <oonf/base/oonf_rfc5444.h>
53 #include <oonf/librfc5444/rfc5444_reader.h>
54 #include <oonf/librfc5444/rfc5444_writer.h>
55
56 #include <oonf/crypto/rfc5444_signature/rfc5444_signature.h>
57
58 #define LOG_RFC5444_SIG _rfc5444_sig_subsystem.logging
59
60 /* prototypes */
61 static int _init(void);
62 static void _cleanup(void);
63 static enum rfc5444_result _cb_signature_tlv(struct rfc5444_reader_tlvblock_context *context);
64 static int _cb_add_signature(struct rfc5444_writer_postprocessor *processor, struct rfc5444_writer_target *target,
65   struct rfc5444_writer_message *msg, uint8_t *data, size_t *data_size);
66
67 static size_t _remove_signature_data(uint8_t *dst, const struct rfc5444_reader_tlvblock_context *context);
68
69 static void _cb_hash_added(void *ptr);
70 static void _cb_hash_removed(void *ptr);
71 static void _cb_crypt_added(void *ptr);
72 static void _cb_crypt_removed(void *ptr);
73 static void _handle_postprocessor(struct rfc5444_signature *sig);
74
75 static int _avl_cmp_signatures(const void *, const void *);
76 static const void *_cb_get_empty_keyid(struct rfc5444_signature *sig, size_t *len);
77 static enum rfc5444_sigid_check _cb_sigid_okay(struct rfc5444_signature *sig, const void *id, size_t len);
78
79 static bool _cb_is_matching_signature(struct rfc5444_writer_postprocessor *processor, int msg_type);
80
81 /* plugin declaration */
82 static const char *_dependencies[] = {
83   OONF_CLASS_SUBSYSTEM,
84   OONF_RFC5444_SUBSYSTEM,
85   OONF_RFC7182_PROVIDER_SUBSYSTEM,
86 };
87 static struct oonf_subsystem _rfc5444_sig_subsystem = {
88   .name = OONF_RFC5444_SIG_SUBSYSTEM,
89   .dependencies = _dependencies,
90   .dependencies_count = ARRAYSIZE(_dependencies),
91   .descr = "OONF rfc5444 signature plugin",
92   .author = "Henning Rogge",
93
94   .init = _init,
95   .cleanup = _cleanup,
96 };
97 DECLARE_OONF_PLUGIN(_rfc5444_sig_subsystem);
98
99 /* tlvblock consumer for signature TLVs */
100 static struct rfc5444_reader_tlvblock_consumer _signature_msg_consumer = {
101   .order = RFC5444_VALIDATOR_PRIORITY,
102   .default_msg_consumer = true,
103
104   .block_callback = _cb_signature_tlv,
105 };
106
107 static struct rfc5444_reader_tlvblock_consumer _signature_pkt_consumer = {
108   .order = RFC5444_VALIDATOR_PRIORITY,
109   .block_callback = _cb_signature_tlv,
110 };
111
112 static struct rfc5444_reader_tlvblock_consumer_entry _pkt_signature_tlv = {
113   .type = RFC7182_PKTTLV_ICV,
114 };
115
116 static struct rfc5444_reader_tlvblock_consumer_entry _msg_signature_tlv = {
117   .type = RFC7182_MSGTLV_ICV,
118 };
119
120 static struct oonf_rfc5444_protocol *_protocol;
121
122 /* tree of registered signatures */
123 static struct avl_tree _sig_tree;
124
125 /* static buffer for signature calculation */
126 static uint8_t _static_message_buffer[RFC5444_MAX_PACKET_SIZE];
127 static uint8_t _crypt_buffer[RFC5444_MAX_PACKET_SIZE];
128
129 /* listeners for crypto and hash algorithms */
130 static struct oonf_class_extension _hash_listener = {
131   .ext_name = "rfc5444 signatures",
132   .class_name = OONF_RFC7182_HASH_CLASS,
133   .cb_add = _cb_hash_added,
134   .cb_remove = _cb_hash_removed,
135 };
136 static struct oonf_class_extension _crypt_listener = {
137   .ext_name = "rfc5444 signatures",
138   .class_name = OONF_RFC7182_CRYPTO_CLASS,
139   .cb_add = _cb_crypt_added,
140   .cb_remove = _cb_crypt_removed,
141 };
142
143 /**
144  * Constructor of subsystem
145  * @return -1 if rfc5444 protocol was not available, 0 otherwise
146  */
147 static int
148 _init(void) {
149   _protocol = oonf_rfc5444_add_protocol(RFC5444_PROTOCOL, true);
150   if (_protocol == NULL) {
151     return -1;
152   }
153
154   rfc5444_reader_add_message_consumer(&_protocol->reader, &_signature_msg_consumer, &_msg_signature_tlv, 1);
155   rfc5444_reader_add_packet_consumer(&_protocol->reader, &_signature_pkt_consumer, &_pkt_signature_tlv, 1);
156   avl_init(&_sig_tree, _avl_cmp_signatures, true);
157
158   oonf_class_extension_add(&_hash_listener);
159   oonf_class_extension_add(&_crypt_listener);
160   return 0;
161 }
162
163 /**
164  * Destructor of subsystem
165  */
166 static void
167 _cleanup(void) {
168   struct rfc5444_signature *sig, *sig_it;
169
170   avl_for_each_element_safe(&_sig_tree, sig, _node, sig_it) {
171     rfc5444_sig_remove(sig);
172   }
173
174   rfc5444_reader_remove_message_consumer(&_protocol->reader, &_signature_msg_consumer);
175   rfc5444_reader_remove_packet_consumer(&_protocol->reader, &_signature_pkt_consumer);
176   oonf_rfc5444_remove_protocol(_protocol);
177
178   oonf_class_extension_remove(&_hash_listener);
179   oonf_class_extension_remove(&_crypt_listener);
180 }
181
182 /**
183  * Add a message signature
184  * @param sig pointer to signature definition
185  */
186 void
187 rfc5444_sig_add(struct rfc5444_signature *sig) {
188   sig->_node.key = &sig->key;
189
190   if (sig->verify_id == NULL) {
191     sig->verify_id = _cb_sigid_okay;
192   }
193   if (sig->getKeyId == NULL) {
194     sig->getKeyId = _cb_get_empty_keyid;
195   }
196
197   avl_insert(&_sig_tree, &sig->_node);
198
199   /* initialize postprocessor */
200   sig->_postprocessor.priority = 0;
201   sig->_postprocessor.process = _cb_add_signature;
202   sig->_postprocessor.is_matching_signature = _cb_is_matching_signature;
203
204   /* try to get hash/crypto */
205   sig->hash = rfc7182_get_hash(sig->key.hash_function);
206   sig->crypt = rfc7182_get_crypt(sig->key.crypt_function);
207
208   _handle_postprocessor(sig);
209 }
210
211 /**
212  * Remove a signature
213  * @param sig pointer to signature definition
214  */
215 void
216 rfc5444_sig_remove(struct rfc5444_signature *sig) {
217   rfc5444_writer_unregister_postprocessor(&_protocol->writer, &sig->_postprocessor);
218   avl_remove(&_sig_tree, &sig->_node);
219 }
220
221 /**
222  * Callback for checking both message and packet signature TLVs
223  * @param context rfc5444 TLV context
224  * @return okay or drop
225  */
226 static enum rfc5444_result
227 _cb_signature_tlv(struct rfc5444_reader_tlvblock_context *context) {
228   struct rfc5444_reader_tlvblock_consumer_entry *sig_tlv;
229   struct rfc5444_reader_tlvblock_entry *tlv;
230   struct rfc5444_signature *sig, *sigstart;
231   struct rfc5444_signature_key sigkey;
232   enum rfc5444_sigid_check check;
233   enum rfc5444_result drop_value;
234   int msg_type;
235   uint8_t key_id_len;
236   uint8_t *static_data;
237   size_t static_length, key_length;
238   const void *key;
239   bool sig_to_verify;
240 #ifdef OONF_LOG_DEBUG_INFO
241   struct netaddr_str nbuf;
242 #endif
243
244   if (context->type == RFC5444_CONTEXT_PACKET) {
245     msg_type = RFC5444_WRITER_PKT_POSTPROCESSOR;
246     drop_value = RFC5444_DROP_PACKET;
247     sig_tlv = &_pkt_signature_tlv;
248   }
249   else {
250     msg_type = context->msg_type;
251     drop_value = RFC5444_DROP_MESSAGE;
252     sig_tlv = &_msg_signature_tlv;
253   }
254
255   /* initialize verification fields */
256   sig_to_verify = false;
257   avl_for_each_element(&_sig_tree, sig, _node) {
258     bool match = sig->is_matching_signature(sig, msg_type);
259
260     sig->_must_be_verified = sig->drop_if_invalid && match;
261     sig->verified = false;
262
263     sig_to_verify |= match;
264   }
265
266   if (!sig_to_verify) {
267     /* nothing to do, no matching signature */
268     return RFC5444_OKAY;
269   }
270
271   OONF_DEBUG(LOG_RFC5444_SIG, "Start checking signature for message type %d", msg_type);
272
273   for (tlv = sig_tlv->tlv; tlv; tlv = tlv->next_entry) {
274     if (tlv->type_ext != RFC7182_ICV_EXT_CRYPTHASH && tlv->type_ext != RFC7182_ICV_EXT_SRCSPEC_CRYPTHASH) {
275       /* unknown subtype, just ignore */
276       OONF_INFO(LOG_RFC5444_SIG, "Signature with unknown ext-type: %u", tlv->type_ext);
277       continue;
278     }
279     if (tlv->length < 4) {
280       /* not enough bytes for valid signature */
281       OONF_INFO(LOG_RFC5444_SIG, "Signature tlv too short: %u bytes", tlv->length);
282       continue;
283     }
284
285     sigkey.hash_function = tlv->single_value[0];
286     sigkey.crypt_function = tlv->single_value[1];
287     key_id_len = tlv->single_value[2];
288
289     if (tlv->length <= 3 + key_id_len) {
290       /* not enouOONF_LOG_DEBUG_INFOgh bytes for valid signature */
291       OONF_INFO_HEX(LOG_RFC5444_SIG, tlv->single_value, tlv->length, "Signature tlv %u/%u too short: %u bytes",
292         tlv->single_value[0], tlv->single_value[1], tlv->length);
293       continue;
294     }
295
296     /* assemble static message buffer */
297     if (tlv->type_ext == RFC7182_ICV_EXT_SRCSPEC_CRYPTHASH) {
298       OONF_DEBUG(LOG_RFC5444_SIG, "incoming src IP: %s", netaddr_to_string(&nbuf, _protocol->input_address));
299
300       /* copy source address into buffer */
301       netaddr_to_binary(_static_message_buffer, _protocol->input_address, sizeof(_static_message_buffer));
302       static_length = netaddr_get_binlength(_protocol->input_address);
303     }
304     else {
305       static_length = 0;
306     }
307     memcpy(&_static_message_buffer[static_length], tlv->single_value, 3 + key_id_len);
308     static_length += 3 + key_id_len;
309
310     static_data = &_static_message_buffer[static_length];
311     static_length += _remove_signature_data(static_data, context);
312
313     /* loop over all possible signatures */
314     avl_for_each_elements_with_key(&_sig_tree, sig, _node, sigstart, &sigkey) {
315       if (!sig->is_matching_signature(sig, msg_type)) {
316         /* signature doesn't apply to this message type */
317         continue;
318       }
319
320       if ((tlv->type_ext == RFC7182_ICV_EXT_SRCSPEC_CRYPTHASH) != sig->source_specific) {
321         OONF_INFO(LOG_RFC5444_SIG, "Signature extension %u does not match", tlv->type_ext);
322         continue;
323       }
324
325       /* see how signature want to handle the incoming key id */
326       check = sig->verify_id(sig, &tlv->single_value[3], key_id_len);
327       if (check == RFC5444_SIGID_IGNORE) {
328         /* signature wants to ignore this TLV */
329         continue;
330       }
331       if (check == RFC5444_SIGID_DROP) {
332         /* signature wants us to drop this context */
333         OONF_INFO(LOG_RFC5444_SIG, "Dropped message because of wrong key-id");
334         return drop_value;
335       }
336
337       /* remember source IP */
338       sig->source = _protocol->input_address;
339
340       /* check signature */
341       key = sig->getCryptoKey(sig, &key_length);
342       sig->verified = sig->crypt->validate(sig->crypt, sig->hash, &tlv->single_value[3 + key_id_len],
343         tlv->length - 3 - key_id_len, _static_message_buffer, static_length, key, key_length);
344
345       OONF_DEBUG(LOG_RFC5444_SIG, "Checked signature hash=%d/crypt=%d: %s", sig->key.hash_function,
346         sig->key.crypt_function, sig->verified ? "check" : "bad");
347     }
348   }
349
350   /* check if mandatory signatures are missing or failed*/
351   avl_for_each_element(&_sig_tree, sig, _node) {
352     if (!sig->verified && sig->_must_be_verified) {
353       OONF_INFO(LOG_RFC5444_SIG, "Dropped %s because bad/missing signature",
354         msg_type == RFC5444_WRITER_PKT_POSTPROCESSOR ? "packet" : "message");
355       return drop_value;
356     }
357   }
358
359   if (sig_to_verify) {
360     OONF_INFO(
361       LOG_RFC5444_SIG, "%s signature valid!", msg_type == RFC5444_WRITER_PKT_POSTPROCESSOR ? "packet" : "message");
362   }
363   return RFC5444_OKAY;
364 }
365
366 /**
367  * Post processor to add a packet signature
368  * @param processor rfc5444 post-processor
369  * @param target rfc5444 target
370  * @param msg rfc5444 message, NULL for packet signature
371  * @param data pointer to binary data
372  * @param length pointer to length of binary data, will be overwritten by function
373  * @return -1 if an error happened, 0 otherwise
374  */
375 static int
376 _cb_add_signature(struct rfc5444_writer_postprocessor *processor, struct rfc5444_writer_target *target,
377   struct rfc5444_writer_message *msg, uint8_t *data, size_t *data_size) {
378   struct rfc5444_signature *sig;
379   struct oonf_rfc5444_target *oonf_target;
380   const union netaddr_socket *local_socket;
381   struct netaddr srcaddr;
382
383   size_t hash_buffer_size, sig_size, sig_tlv_size, tlvblock_size, key_size;
384   uint8_t *tlvblock;
385   int idx;
386
387   size_t crypt_len;
388
389   const void *key_id, *key;
390   size_t key_id_length;
391
392 #ifdef OONF_LOG_DEBUG_INFO
393   struct netaddr_str nbuf;
394 #endif
395
396   sig = container_of(processor, struct rfc5444_signature, _postprocessor);
397
398   if (!msg) {
399     OONF_INFO(LOG_RFC5444_SIG, "Add signature data to packet");
400   }
401   else {
402     OONF_INFO(LOG_RFC5444_SIG, "Add signature data to message %u", msg->type);
403   }
404   oonf_target = oonf_rfc5444_get_target_from_rfc5444_target(target);
405
406   /*
407    * copy data into static buffer, leave space in front
408    * for source address and signature data
409    */
410   if (sig->source_specific) {
411     local_socket = oonf_rfc5444_target_get_local_socket(oonf_target);
412     if (netaddr_from_socket(&srcaddr, local_socket)) {
413       return -1;
414     }
415     OONF_DEBUG(LOG_RFC5444_SIG, "outgoing src IP: %s", netaddr_to_string(&nbuf, &srcaddr));
416
417     netaddr_to_binary(_static_message_buffer, &srcaddr, sizeof(_static_message_buffer));
418     idx = netaddr_get_binlength(&srcaddr);
419   }
420   else {
421     idx = 0;
422   }
423
424   key_id_length = 0;
425   key_id = sig->getKeyId(sig, &key_id_length);
426
427   _static_message_buffer[idx++] = sig->key.hash_function;
428   _static_message_buffer[idx++] = sig->key.crypt_function;
429   _static_message_buffer[idx++] = key_id_length;
430   memcpy(&_static_message_buffer[idx], key_id, key_id_length);
431   idx += key_id_length;
432   hash_buffer_size = idx + *data_size;
433
434   if (msg) {
435     /* just copy message data, it always has a TLV block */
436     memcpy(&_static_message_buffer[idx], data, *data_size);
437
438     /*
439      * get pointer to message tlvblock
440      * zero hoplimit/hopcount in static buffer
441      */
442     idx += 4;
443     tlvblock = &data[4];
444     if (msg->has_origaddr) {
445       idx += _protocol->writer.msg_addr_len;
446       tlvblock += _protocol->writer.msg_addr_len;
447     }
448     if (msg->has_hoplimit) {
449       _static_message_buffer[idx++] = 0;
450       tlvblock++;
451     }
452     if (msg->has_hopcount) {
453       _static_message_buffer[idx++] = 0;
454       tlvblock++;
455     }
456     if (msg->has_seqno) {
457       idx += 2;
458       tlvblock += 2;
459     }
460   }
461   else {
462     /* just copy packet for hashing */
463     memcpy(&_static_message_buffer[idx], data, *data_size);
464
465     if (data[0] & RFC5444_PKT_FLAG_SEQNO) {
466       tlvblock = &data[3];
467     }
468     else {
469       tlvblock = &data[1];
470     }
471   }
472
473   /* calculate encrypted hash value */
474   crypt_len = sizeof(_crypt_buffer);
475   key = sig->getCryptoKey(sig, &key_size);
476   if (sig->crypt->sign(
477         sig->crypt, sig->hash, _crypt_buffer, &crypt_len, _static_message_buffer, hash_buffer_size, key, key_size)) {
478     OONF_WARN(LOG_RFC5444_SIG, "Signature generation failed");
479     return -1;
480   }
481
482   if (crypt_len > sig->crypt->getSignSize(sig->crypt, sig->hash)) {
483     OONF_WARN(LOG_RFC5444_SIG,
484       "Signature too long: "
485       "%" PRINTF_SIZE_T_SPECIFIER " > %" PRINTF_SIZE_T_SPECIFIER,
486       crypt_len, sig->crypt->getSignSize(sig->crypt, sig->hash));
487     return -1;
488   }
489
490   /* calulate signature size */
491   sig_size = 3 + key_id_length + crypt_len;
492
493   /* tlv with type extension and (extended) value */
494   sig_tlv_size = 4 + sig_size;
495   if (sig_size > 255) {
496     sig_tlv_size++;
497   }
498
499   if (msg == NULL && (data[0] & RFC5444_PKT_FLAG_TLV) == 0) {
500     /* mark packet as "has tlv" */
501     data[0] |= RFC5444_PKT_FLAG_TLV;
502
503     /* add space for signature tlv and tlv block */
504     memmove(tlvblock + 2 + sig_tlv_size, tlvblock, *data_size - (tlvblock - data));
505
506     /* add two bytes for new tlv-block header */
507     *data_size += 2;
508
509     /* clear new packet tlvblock length */
510     tlvblock[0] = 0;
511     tlvblock[1] = 0;
512   }
513   else {
514     /* add space for signature tlv */
515     memmove(tlvblock + 2 + sig_tlv_size, tlvblock + 2, *data_size - (tlvblock + 2 - data));
516   }
517   /* write new tlvblock size */
518   tlvblock_size = (256 * tlvblock[0] + tlvblock[1]);
519   tlvblock_size += sig_tlv_size;
520   *tlvblock++ = tlvblock_size / 256;
521   *tlvblock++ = tlvblock_size & 255;
522
523   /* write signature TLV header */
524   *tlvblock++ = RFC7182_MSGTLV_ICV;
525   if (sig_size > 255) {
526     *tlvblock++ = RFC5444_TLV_FLAG_TYPEEXT | RFC5444_TLV_FLAG_VALUE | RFC5444_TLV_FLAG_EXTVALUE;
527   }
528   else {
529     *tlvblock++ = RFC5444_TLV_FLAG_TYPEEXT | RFC5444_TLV_FLAG_VALUE;
530   }
531
532   if (sig->source_specific) {
533     *tlvblock++ = RFC7182_ICV_EXT_SRCSPEC_CRYPTHASH;
534   }
535   else {
536     *tlvblock++ = RFC7182_ICV_EXT_CRYPTHASH;
537   }
538
539   if (sig_size > 255) {
540     *tlvblock++ = sig_size / 256;
541     *tlvblock++ = sig_size & 255;
542   }
543   else {
544     *tlvblock++ = sig_size;
545   }
546
547   /* write signature tlv value */
548   *tlvblock++ = sig->key.hash_function;
549   *tlvblock++ = sig->key.crypt_function;
550   *tlvblock++ = key_id_length;
551   memcpy(tlvblock, key_id, key_id_length);
552   memcpy(tlvblock + key_id_length, _crypt_buffer, crypt_len);
553
554   /* fix data size */
555   *data_size += sig_tlv_size;
556
557   if (msg) {
558     /* fix message size field */
559     data[2] = *data_size / 256;
560     data[3] = *data_size & 255;
561   }
562
563   /* return new message size */
564   OONF_DEBUG_HEX(LOG_RFC5444_SIG, data, *data_size, "Signed data:");
565
566   return 0;
567 }
568
569 /**
570  * Remove signature TLVs from a message/packet
571  * @param dst pointer to destination buffer for unsigned message/packet
572  * @param context rfc5444 context
573  * @return size of unsigned data
574  */
575 static size_t
576 _remove_signature_data(uint8_t *dst, const struct rfc5444_reader_tlvblock_context *context) {
577   const uint8_t *src_ptr, *src_end;
578   uint8_t *dst_ptr, *tlvblock;
579   uint16_t len, hoplimit, hopcount;
580   uint16_t blocklen, tlvlen;
581
582   hoplimit = 0;
583   hopcount = 0;
584
585   /* initialize pointers to src/dst */
586   if (context->type == RFC5444_CONTEXT_PACKET) {
587     src_ptr = context->pkt_buffer;
588     src_end = context->pkt_buffer + context->pkt_size;
589
590     /* calculate message header length */
591     if (context->has_pktseqno) {
592       len = 3;
593     }
594     else {
595       len = 1;
596     }
597   }
598   else {
599     src_ptr = context->msg_buffer;
600     src_end = context->msg_buffer + context->msg_size;
601
602     /* calculate message header length */
603     len = 4;
604     if (context->has_origaddr) {
605       len += context->addr_len;
606     }
607     if (context->has_hoplimit) {
608       hoplimit = len;
609       len++;
610     }
611     if (context->has_hopcount) {
612       hopcount = len;
613       len++;
614     }
615     if (context->has_seqno) {
616       len += 2;
617     }
618   }
619   dst_ptr = dst;
620
621   /* copy pakcet/message header */
622   memcpy(dst_ptr, src_ptr, len);
623
624   /* clear hoplimit/hopcount */
625   if (hoplimit) {
626     dst_ptr[hoplimit] = 0;
627   }
628   if (hopcount) {
629     dst_ptr[hopcount] = 0;
630   }
631
632   /* advance to end of header */
633   src_ptr += len;
634   dst_ptr += len;
635   tlvblock = dst_ptr;
636
637   /* copy all message tlvs except for signature tlvs */
638   blocklen = 256 * src_ptr[0] + src_ptr[1];
639
640   /* skip over message tlv block header, we write the number later */
641   src_ptr += 2;
642   dst_ptr += 2;
643
644   /* loop over message tlvs */
645   len = blocklen;
646   while (len > 0) {
647     /* calculate length of TLV */
648     tlvlen = 2;
649     if (src_ptr[1] & RFC5444_TLV_FLAG_TYPEEXT) {
650       /* extended type, one extra byte */
651       tlvlen++;
652     }
653     if (src_ptr[1] & RFC5444_TLV_FLAG_VALUE) {
654       /* TLV has a value field */
655       if (src_ptr[1] & RFC5444_TLV_FLAG_EXTVALUE) {
656         /* 2-byte value */
657         tlvlen += (256 * src_ptr[tlvlen]) + src_ptr[tlvlen + 1] + 2;
658       }
659       else {
660         /* 1-byte value */
661         tlvlen += src_ptr[tlvlen] + 1;
662       }
663     }
664
665     if (src_ptr[0] != RFC7182_MSGTLV_ICV) {
666       /* copy TLV if not signature TLV */
667       memcpy(dst_ptr, src_ptr, tlvlen);
668       dst_ptr += tlvlen;
669     }
670     else {
671       /* reduce blocklength */
672       blocklen -= tlvlen;
673     }
674     len -= tlvlen;
675     src_ptr += tlvlen;
676   }
677
678   if (blocklen > 0 || context->type == RFC5444_CONTEXT_MESSAGE) {
679     /* overwrite message tlvblock length */
680     tlvblock[0] = blocklen / 256;
681     tlvblock[1] = blocklen & 255;
682   }
683   else {
684     /* remove empty packet tlvblock and fix flags */
685     dst_ptr -= 2;
686     dst[0] &= ~RFC5444_PKT_FLAG_TLV;
687   }
688
689   /* copy rest of data */
690   len = src_end - src_ptr;
691   memcpy(dst_ptr, src_ptr, len);
692
693   /* calculate data length */
694   len = dst_ptr - dst + len;
695   if (context->type == RFC5444_CONTEXT_MESSAGE) {
696     /* overwrite message length */
697     dst[2] = len / 256;
698     dst[3] = len & 255;
699   }
700   return len;
701 }
702
703 static void
704 _cb_hash_added(void *ptr) {
705   struct rfc7182_hash *hash = ptr;
706   struct rfc5444_signature *sig;
707
708   avl_for_each_element(&_sig_tree, sig, _node) {
709     if (sig->key.hash_function == hash->type && sig->hash == NULL) {
710       sig->hash = hash;
711
712       _handle_postprocessor(sig);
713     }
714   }
715 }
716
717 static void
718 _cb_hash_removed(void *ptr) {
719   struct rfc7182_hash *hash = ptr;
720   struct rfc5444_signature *sig;
721
722   avl_for_each_element(&_sig_tree, sig, _node) {
723     if (sig->key.hash_function == hash->type && sig->hash != NULL) {
724       sig->hash = NULL;
725
726       _handle_postprocessor(sig);
727     }
728   }
729 }
730
731 static void
732 _cb_crypt_added(void *ptr) {
733   struct rfc7182_crypt *crypt = ptr;
734   struct rfc5444_signature *sig;
735
736   avl_for_each_element(&_sig_tree, sig, _node) {
737     if (sig->key.crypt_function == crypt->type && sig->crypt == NULL) {
738       sig->crypt = crypt;
739
740       _handle_postprocessor(sig);
741     }
742   }
743 }
744
745 static void
746 _cb_crypt_removed(void *ptr) {
747   struct rfc7182_crypt *crypt = ptr;
748   struct rfc5444_signature *sig;
749
750   avl_for_each_element(&_sig_tree, sig, _node) {
751     if (sig->key.crypt_function == crypt->type && sig->crypt != NULL) {
752       sig->crypt = NULL;
753
754       _handle_postprocessor(sig);
755     }
756   }
757 }
758
759 /**
760  * Helper function that registers the packet post-processors
761  * for signature schemes that referred to an unregistered
762  * hash/crypto-function
763  */
764 static void
765 _handle_postprocessor(struct rfc5444_signature *sig) {
766   bool registered;
767
768   registered = avl_is_node_added(&sig->_postprocessor._node);
769
770   if (!registered && sig->hash != NULL && sig->crypt != NULL) {
771     sig->_postprocessor.allocate_space = sig->crypt->getSignSize(sig->crypt, sig->hash);
772     rfc5444_writer_register_postprocessor(&_protocol->writer, &sig->_postprocessor);
773   }
774   else if (registered && (sig->hash == NULL || sig->crypt == NULL)) {
775     rfc5444_writer_unregister_postprocessor(&_protocol->writer, &sig->_postprocessor);
776   }
777 }
778
779 /**
780  * AVL comparator for two signature keys
781  * @param k1
782  * @param k2
783  * @return
784  */
785 static int
786 _avl_cmp_signatures(const void *k1, const void *k2) {
787   return memcmp(k1, k2, sizeof(struct rfc5444_signature_key));
788 }
789
790 /**
791  * Callback to query the key-id including length
792  * @param sig this rfc5444 signature
793  * @param len pointer to length, will set by this function
794  * @return pointer to key-id
795  */
796 static const void *
797 _cb_get_empty_keyid(struct rfc5444_signature *sig __attribute__((unused)), size_t *len) {
798   static const char *id = "";
799
800   *len = 0;
801   return id;
802 }
803
804 /**
805  * Callback to checks if the key-id is valid for a signature before
806  * checking the signature itself. This can also be used to store the key-id
807  * before 'crypt' callback is called.
808  * @param sig this rfc5444 signature
809  * @param id pointer to key-id
810  * @param len length of key id
811  * @return okay, ignore or drop
812  */
813 static enum rfc5444_sigid_check
814 _cb_sigid_okay(struct rfc5444_signature *sig __attribute__((unused)), const void *id __attribute((unused)),
815   size_t len __attribute((unused))) {
816   return RFC5444_SIGID_OKAY;
817 }
818
819 static bool
820 _cb_is_matching_signature(struct rfc5444_writer_postprocessor *processor, int msg_type) {
821   struct rfc5444_signature *sig;
822
823   sig = container_of(processor, struct rfc5444_signature, _postprocessor);
824
825   return sig->is_matching_signature(sig, msg_type);
826 }