source: trunk/packages/xen-3.1/xen-3.1/tools/vnet/vnet-module/esp.c @ 34

Last change on this file since 34 was 34, checked in by hartmans, 18 years ago

Add xen and xen-common

File size: 26.1 KB
Line 
1/*
2 * Copyright (C) 2004, 2005 Mike Wray <mike.wray@hp.com>
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by the
6 * Free Software Foundation; either version 2 of the License, or (at your
7 * option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
11 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
12 * for more details.
13 *
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free software Foundation, Inc.,
16 * 59 Temple Place, suite 330, Boston, MA 02111-1307 USA
17 *
18 */
19#include <linux/config.h>
20#include <linux/module.h>
21#include <linux/types.h>
22#include <linux/sched.h>
23#include <linux/kernel.h>
24#include <asm/uaccess.h>
25
26#include <linux/init.h>
27
28#include <linux/version.h>
29
30#include <linux/skbuff.h>
31#include <linux/netdevice.h>
32#include <linux/net.h>
33#include <linux/in.h>
34#include <linux/inet.h>
35
36#include <net/ip.h>
37#include <net/protocol.h>
38#include <net/route.h>
39
40#include <linux/if_ether.h>
41#include <linux/icmp.h>
42
43#include <asm/scatterlist.h>
44#include <linux/crypto.h>
45#include <linux/pfkeyv2.h>
46#include <linux/random.h>
47
48#include <esp.h>
49#include <sa.h>
50#include <sa_algorithm.h>
51#include <tunnel.h>
52#include <vnet.h>
53#include <skb_util.h>
54#include <skb_context.h>
55
56static const int DEBUG_ICV = 0;
57
58#define MODULE_NAME "IPSEC"
59#define DEBUG 1
60#undef DEBUG
61#include "debug.h"
62
63#ifndef CONFIG_CRYPTO_HMAC
64#warning No esp transform - CONFIG_CRYPTO_HMAC not defined
65
66int __init esp_module_init(void){
67    return 0;
68}
69
70void __exit esp_module_exit(void){
71}
72
73#else
74
75/* Outgoing packet:                            [ eth | ip | data ]
76 * After etherip:        [ eth2 | ip2 |  ethip | eth | ip | data ]
77 * After esp   :   [ eth2 | ip2 | esp | {ethip | eth | ip | data} | pad | icv ]
78 *                                                        ^     +
79 * The curly braces { ... } denote encryption.
80 * The esp header includes the fixed esp headers and the iv (variable size).
81 * The point marked ^ does not move. To the left is in the header, to the right
82 * is in the frag. Remember that all outgoing skbs (from domains) have 1 frag.
83 * Data after + is added by esp, using an extra frag.
84 *
85 * Incoming as above.
86 * After decrypt:  [ eth2 | ip2 | esp |  ethip | eth | ip | data  | pad | icv ]
87 * Trim tail:      [ eth2 | ip2 | esp |  ethip | eth | ip | data ]
88 * Drop hdr:             [ eth2 | ip2 |  ethip | eth | ip | data ]
89 *                                    ^
90 * The point marked ^ does not move. Incoming skbs are linear (no frags).
91 * The tail is trimmed by adjusting skb->tail and len.
92 * The esp hdr is dropped by using memmove to move the headers and
93 * adjusting the skb pointers.
94 *
95 * todo: Now this code is in linux we can't assume 1 frag for outbound skbs,
96 * or (maybe) that memmove is safe on inbound.
97 */
98
99/** Round n up to a multiple of block.
100 * If block is less than 2 does nothing.
101 * Otherwise assume block is a power of 2.
102 *
103 * @param n to round up
104 * @param block size to round to a multiple of
105 * @return rounded value
106 */
107static inline int roundupto(int n, int block){
108    if(block <= 1) return n;
109    block--;
110    return (n + block) & ~block;
111}
112
113/** Check if n is a multiple of block.
114 * If block is less than 2 returns 1.
115 * Otherwise assumes block is a power of 2.
116 *
117 * @param n to check
118 * @param block block size
119 * @return 1 if a multiple, 0 otherwise
120 */
121static inline int multipleof(int n, int block){
122    if(block <= 1) return 1;
123    block--;
124    return !(n & block);
125}
126
127/** Convert from bits to bytes.
128 *
129 * @param n number of bits
130 * @return number of bytes
131 */
132static inline int bits_to_bytes(int n){
133    return n / 8;
134}
135
136
137/** Insert esp padding at the end of an skb.
138 * Inserts padding bytes, number of padding bytes, protocol number.
139 *
140 * @param skb skb
141 * @param offset offset from skb end to where padding should end
142 * @param extra_n total amount of padding
143 * @param protocol protocol number (from original ip hdr)
144 * @return 0 on success, error code otherwise
145 */
146static int esp_sa_pad(struct sk_buff *skb, int offset, int extra_n,
147                      unsigned char protocol){
148    int err;
149    char *data;
150    int pad_n = extra_n - ESP_PAD_N;
151    int i;
152    char buf[extra_n];
153
154    data = buf;
155    for(i = 1; i <= pad_n; i++){
156        *data++ = i;
157    }
158    *data++ = pad_n;
159    *data++ = protocol;
160    err = skb_put_bits(skb, skb->len - offset - extra_n, buf, extra_n);
161    return err;
162}
163
164/** Encrypt skb. Skips esp header and iv.
165 * Assumes skb->data points at esp header.
166 *
167 * @param esp esp state
168 * @parm esph esp header
169 * @param skb packet
170 * @param head_n size of esp header and iv
171 * @param iv_n size of iv
172 * @param text_n size of ciphertext
173 * @return 0 on success, error code otherwise
174 */
175static int esp_sa_encrypt(ESPState *esp, ESPHdr *esph, struct sk_buff *skb,
176                   int head_n, int iv_n, int text_n){
177    int err = 0;
178    int sg_n = skb_shinfo(skb)->nr_frags + 1;
179    struct scatterlist sg[sg_n];
180
181    err = skb_scatterlist(skb, sg, &sg_n, head_n, text_n);
182    if(err) goto exit;
183    if(iv_n){
184        crypto_cipher_set_iv(esp->cipher.tfm, esp->cipher.iv, iv_n);
185    }
186    crypto_cipher_encrypt(esp->cipher.tfm, sg, sg, text_n);
187    if(iv_n){
188        memcpy(esph->data, esp->cipher.iv, iv_n);
189        crypto_cipher_get_iv(esp->cipher.tfm, esp->cipher.iv, iv_n);
190    }
191  exit:
192    return err;
193}
194
195/** Decrypt skb. Skips esp header and iv.
196 * Assumes skb->data points at esp header.
197 *
198 * @param esp esp state
199 * @parm esph esp header
200 * @param skb packet
201 * @param head_n size of esp header and iv
202 * @param iv_n size of iv
203 * @param text_n size of ciphertext
204 * @return 0 on success, error code otherwise
205 */
206static int esp_sa_decrypt(ESPState *esp, ESPHdr *esph, struct sk_buff *skb,
207                   int head_n, int iv_n, int text_n){
208    int err = 0;
209    int sg_n = skb_shinfo(skb)->nr_frags + 1;
210    struct scatterlist sg[sg_n];
211
212    err = skb_scatterlist(skb, sg, &sg_n, head_n, text_n);
213    if(err) goto exit;
214    if(iv_n){
215        crypto_cipher_set_iv(esp->cipher.tfm, esph->data, iv_n);
216    }
217    crypto_cipher_decrypt(esp->cipher.tfm, sg, sg, text_n);
218  exit:
219    return err;
220}
221
222/** Compute icv. Includes esp header, iv and ciphertext.
223 * Assumes skb->data points at esp header.
224 *
225 * @param esp esp state
226 * @param skb packet
227 * @param digest_n number of bytes to digest
228 * @param icv_n size of icv
229 * @return 0 on success, error code otherwise
230 */
231static int esp_sa_digest(ESPState *esp, struct sk_buff *skb, int digest_n, int icv_n){
232    int err = 0;
233    u8 icv[icv_n];
234   
235    if(DEBUG_ICV){
236        dprintf("> skb digest_n=%d icv_n=%d\n", digest_n, icv_n);
237        skb_print_bits("esp", skb, 0, digest_n);
238    }
239    memset(icv, 0, icv_n);
240    esp->digest.icv(esp, skb, 0, digest_n, icv);
241    skb_put_bits(skb, digest_n, icv, icv_n);
242    return err;
243}
244
245/** Check the icv and trim it from the skb tail.
246 *
247 * @param sa sa state
248 * @param esp esp state
249 * @param esph esp header
250 * @param skb packet
251 * @return 0 on success, error code otherwise
252 */
253static int esp_check_icv(SAState *sa, ESPState *esp, ESPHdr *esph, struct sk_buff *skb){
254    int err = 0;
255    int icv_n = esp->digest.icv_n;
256    int digest_n = skb->len - icv_n;
257    u8 icv_skb[icv_n];
258    u8 icv_new[icv_n];
259
260    dprintf(">\n");
261    if(DEBUG_ICV){
262        dprintf("> skb len=%d digest_n=%d icv_n=%d\n",
263                skb->len, digest_n, icv_n);
264        skb_print_bits("esp", skb, 0, skb->len);
265    }
266    if(skb_copy_bits(skb, digest_n, icv_skb, icv_n)){
267        wprintf("> Error getting icv from skb\n");
268        goto exit;
269    }
270    esp->digest.icv(esp, skb, 0, digest_n, icv_new);
271    if(DEBUG_ICV){
272        dprintf("> len=%d icv_n=%d", digest_n, icv_n);
273        printk("\nskb="); buf_print(icv_skb, icv_n);
274        printk("new="); buf_print(icv_new, icv_n);
275    }
276    if(unlikely(memcmp(icv_new, icv_skb, icv_n))){
277        wprintf("> ICV check failed!\n");
278        err = -EINVAL;
279        sa->counts.integrity_failures++;
280        goto exit;
281    }
282    skb_trim_tail(skb, icv_n);
283  exit:
284    dprintf("< err=%d\n", err);
285    return err;
286}
287
288/** Send a packet via an ESP SA.
289 *
290 * @param sa SA state
291 * @param skb packet to send
292 * @param tunnel underlying tunnel
293 * @return 0 on success, negative error code otherwise
294 */
295static int esp_sa_send(SAState *sa, struct sk_buff *skb, Tunnel *tunnel){
296    int err = 0;
297    int ip_n;           // Size of ip header.
298    int plaintext_n;    // Size of plaintext.
299    int ciphertext_n;   // Size of ciphertext (including padding).
300    int extra_n;        // Extra bytes needed for ciphertext.
301    int icv_n = 0;      // Size of integrity check value (icv).
302    int iv_n = 0;       // Size of initialization vector (iv).
303    int head_n;         // Size of esp header and iv.
304    int tail_n;         // Size of esp trailer: padding and icv.
305    ESPState  *esp;
306    ESPHdr *esph;
307
308    dprintf(">\n");
309    esp = sa->data;
310    ip_n = (skb->nh.iph->ihl << 2);
311    // Assuming skb->data points at ethernet header, exclude ethernet
312    // header and IP header.
313    plaintext_n = skb->len - ETH_HLEN - ip_n;
314    // Add size of padding fields.
315    ciphertext_n = roundupto(plaintext_n + ESP_PAD_N, esp->cipher.block_n);
316    if(esp->cipher.pad_n > 0){
317        ciphertext_n = roundupto(ciphertext_n, esp->cipher.pad_n);
318    }
319    extra_n = ciphertext_n - plaintext_n;
320    iv_n = esp->cipher.iv_n;
321    icv_n = esp->digest.icv_n;
322    dprintf("> len=%d plaintext=%d ciphertext=%d extra=%d\n",
323            skb->len, plaintext_n, ciphertext_n, extra_n);
324    dprintf("> iv=%d icv=%d\n", iv_n, icv_n);
325    skb_print_bits("iv", skb, 0, skb->len);
326
327    // Add headroom for esp header and iv, tailroom for the ciphertext
328    // and icv.
329    head_n = ESP_HDR_N + iv_n;
330    tail_n = extra_n + icv_n;
331    err = skb_make_room(&skb, skb, head_n, tail_n);
332    if(err) goto exit;
333    dprintf("> skb=%p\n", skb);
334    // Move the headers up to make space for the esp header.  We can
335    // use memmove() since all this data fits in the skb head.
336    // todo: Can't assume this anymore?
337    dprintf("> header push...\n");
338    __skb_push(skb, head_n);
339    if(0 && skb->mac.raw){
340        dprintf("> skb->mac=%p\n", skb->mac.raw);
341        dprintf("> ETH header pull...\n");
342        memmove(skb->data, skb->mac.raw, ETH_HLEN);
343        skb->mac.raw = skb->data; 
344        __skb_pull(skb, ETH_HLEN);
345    }
346    dprintf("> IP header pull...\n");
347    memmove(skb->data, skb->nh.raw, ip_n);
348    skb->nh.raw = skb->data;
349    __skb_pull(skb, ip_n);
350    esph = (void*)skb->data;
351    // Add spi and sequence number.
352    esph->spi = sa->ident.spi;
353    esph->seq = htonl(++sa->replay.send_seq);
354    // Insert the padding bytes: extra bytes less the pad fields
355    // themselves.
356    dprintf("> esp_sa_pad ...\n");
357    esp_sa_pad(skb, icv_n, extra_n, skb->nh.iph->protocol);
358    if(sa->security & SA_CONF){
359        dprintf("> esp_sa_encrypt...\n");
360        err = esp_sa_encrypt(esp, esph, skb, head_n, iv_n, ciphertext_n);
361        if(err) goto exit;
362    }
363    if(icv_n){
364        dprintf("> esp_sa_digest...\n");
365        err = esp_sa_digest(esp, skb, head_n + ciphertext_n, icv_n);
366        if(err) goto exit;
367    }
368    dprintf("> IP header push...\n");
369    __skb_push(skb, ip_n);
370    if(0 && skb->mac.raw){
371        dprintf("> ETH header push...\n");
372        __skb_push(skb, ETH_HLEN);
373    }
374    // Fix ip header. Adjust length field, set protocol, zero
375    // checksum.
376    {
377        // Total packet length (bytes).
378        int tot_len = ntohs(skb->nh.iph->tot_len);
379        tot_len += head_n;
380        tot_len += tail_n;
381        skb->nh.iph->protocol = IPPROTO_ESP;
382        skb->nh.iph->tot_len  = htons(tot_len);
383        skb->nh.iph->check    = 0;
384    }
385    err = Tunnel_send(tunnel, skb);
386  exit:
387    dprintf("< err=%d\n", err);
388    return err;
389}
390
391/** Release an skb context.
392 * Drops the refcount on the SA.
393 *
394 * @param context to free
395 */
396static void esp_context_free_fn(SkbContext *context){
397    SAState *sa;
398    if(!context) return;
399    sa = context->data;
400    if(!sa) return;
401    context->data = NULL;
402    SAState_decref(sa);
403}   
404
405/** Receive a packet via an ESP SA.
406 * Does ESP receive processing (check icv, decrypt), strips
407 * ESP header and re-receives.
408 *
409 * If return 1 the packet has been freed.
410 * If return <= 0 the caller must free.
411 *
412 * @param sa SA
413 * @param skb packet
414 * @return >= 0 on success, negative protocol otherwise
415 */
416static int esp_sa_recv(SAState *sa, struct sk_buff *skb){
417    int err = -EINVAL;
418    int mine = 0;
419    int vnet = 0; //todo: fixme - need to record skb vnet somewhere
420    ESPState *esp;
421    ESPHdr *esph;
422    ESPPadding *pad;
423    int block_n;        // Cipher blocksize.
424    int icv_n;          // Size of integrity check value (icv).
425    int iv_n;           // Size of initialization vector (iv).
426    int text_n;         // Size of text (ciphertext or plaintext).
427    int head_n;         // Size of esp header and iv.
428
429    dprintf("> skb=%p\n", skb);
430    // Assumes skb->data points at esp hdr.
431    esph = (void*)skb->data;
432    esp = sa->data;
433    block_n = crypto_tfm_alg_blocksize(esp->cipher.tfm);
434    icv_n = esp->digest.icv_n;
435    iv_n = esp->cipher.iv_n;
436    head_n = ESP_HDR_N + iv_n;
437    text_n = skb->len - head_n - icv_n;
438    if(text_n < ESP_PAD_N || !multipleof(text_n, block_n)){
439        wprintf("> Invalid size: text_n=%d tfm:block_n=%d esp:block_n=%d\n",
440                text_n, block_n, esp->cipher.block_n);
441        goto exit;
442    }
443    if(icv_n){
444        err = esp_check_icv(sa, esp, esph, skb);
445        if(err) goto exit;
446    }
447    mine = 1;
448    if(sa->security & SA_CONF){
449        err = esp_sa_decrypt(esp, esph, skb, head_n, iv_n, text_n);
450        if(err) goto exit;
451    }
452    // Strip esp header by moving the other headers down.
453    //todo Maybe not safe to do this anymore.
454    memmove(skb->mac.raw + head_n, skb->mac.raw, (skb->data - skb->mac.raw));
455    skb->mac.raw += head_n;
456    skb->nh.raw  += head_n;
457    // Move skb->data back to ethernet header.
458    // Do in 2 moves to ensure offsets are +ve,
459    // since args to skb_pull/skb_push are unsigned.
460    __skb_pull(skb, head_n);
461    __skb_push(skb, skb->data - skb->mac.raw);
462    // After this esph is invalid.
463    esph = NULL;
464    // Trim padding, restore protocol in IP header.
465    pad = skb_trim_tail(skb, ESP_PAD_N);
466    text_n -= ESP_PAD_N;
467    if((pad->pad_n > 255) | (pad->pad_n > text_n)){
468        wprintf("> Invalid padding: pad_n=%d text_n=%d\n", pad->pad_n, text_n);
469        goto exit;
470    }
471    skb_trim_tail(skb, pad->pad_n);
472    skb->nh.iph->protocol = pad->protocol;
473    err = skb_push_context(skb, vnet, sa->ident.addr, IPPROTO_ESP,
474                           sa, esp_context_free_fn);
475    if(err) goto exit;
476    // Increase sa refcount now the skb context refers to it.
477    // Refcount is decreased by esp_context_free_fn.
478    SAState_incref(sa);
479    // Deliver skb to be received by network code.
480    // Not safe to refer to the skb after this.
481    // todo: return -skb->nh.iph->protocol instead?
482    netif_rx(skb);
483  exit:
484    if(mine){
485        if(err < 0){
486            kfree_skb(skb);
487        }
488        err = 1;
489    }
490    dprintf("< skb=%p err=%d\n", skb, err);
491    return err;
492}
493
494/** Estimate the packet size for some data using ESP processing.   
495 *
496 * @param sa ESP SA
497 * @param data_n data size
498 * @return size after ESP processing
499 */
500static u32 esp_sa_size(SAState *sa, int data_n){
501    // Even in transport mode have to round up to blocksize.
502    // Have to add some padding for alignment even if pad_n is zero.
503    ESPState *esp = sa->data;
504   
505    data_n = roundupto(data_n + ESP_PAD_N, esp->cipher.block_n);
506    if(esp->cipher.pad_n > 0){
507        data_n = roundupto(data_n, esp->cipher.pad_n);
508    }
509    data_n += esp->digest.icv_n;
510    //data_n += esp->cipher.iv_n;
511    data_n += ESP_HDR_N;
512    return data_n;
513}
514
515/** Compute an icv using HMAC digest.
516 *
517 * @param esp ESP state
518 * @param skb packet to digest
519 * @param offset offset to start at
520 * @param len number of bytes to digest
521 * @param icv return parameter for ICV
522 * @return 0 on success, negative error code otherwise
523 */
524static inline void esp_hmac_digest(ESPState *esp, struct sk_buff *skb,
525                                   int offset, int len, u8 *icv){
526    int err = 0;
527    struct crypto_tfm *digest = esp->digest.tfm;
528    char *icv_tmp = esp->digest.icv_tmp;
529    int sg_n = skb_shinfo(skb)->nr_frags + 1;
530    struct scatterlist sg[sg_n];
531
532    dprintf("> offset=%d len=%d\n", offset, len);
533    memset(icv, 0, esp->digest.icv_n);
534    if(DEBUG_ICV){
535        dprintf("> key len=%d\n", esp->digest.key_n);
536        printk("\nkey=");
537        buf_print(esp->digest.key,esp->digest.key_n); 
538    }
539    crypto_hmac_init(digest, esp->digest.key, &esp->digest.key_n);
540    err = skb_scatterlist(skb, sg, &sg_n, offset, len);
541    crypto_hmac_update(digest, sg, sg_n);
542    crypto_hmac_final(digest, esp->digest.key, &esp->digest.key_n, icv_tmp);
543    if(DEBUG_ICV){
544        dprintf("> digest len=%d ", esp->digest.icv_n);
545        printk("\nval=");
546        buf_print(icv_tmp, esp->digest.icv_n);
547    }
548    memcpy(icv, icv_tmp, esp->digest.icv_n);
549    dprintf("<\n");
550}
551
552/** Finish up an esp state.
553 * Releases the digest, cipher, iv and frees the state.
554 *
555 * @parma esp state
556 */
557static void esp_fini(ESPState *esp){
558    if(!esp) return;
559    if(esp->digest.tfm){
560        crypto_free_tfm(esp->digest.tfm);
561        esp->digest.tfm = NULL; 
562    }
563    if(esp->digest.icv_tmp){
564        kfree(esp->digest.icv_tmp);
565        esp->digest.icv_tmp = NULL;
566    }
567    if(esp->cipher.tfm){
568        crypto_free_tfm(esp->cipher.tfm);
569        esp->cipher.tfm = NULL;
570    }
571    if(esp->cipher.iv){
572        kfree(esp->cipher.iv);
573        esp->cipher.iv = NULL;
574    }
575    kfree(esp);
576}
577
578/** Release an ESP SA.
579 *
580 * @param sa ESO SA
581 */
582static void esp_sa_fini(SAState *sa){
583    ESPState *esp;
584    if(!sa) return;
585    esp = sa->data;
586    if(!esp) return;
587    esp_fini(esp);
588    sa->data = NULL;
589}
590
591/** Initialize the cipher for an ESP SA.
592 *
593 * @param sa ESP SA
594 * @param esp ESP state
595 * @return 0 on success, negative error code otherwise
596 */
597static int esp_cipher_init(SAState *sa, ESPState *esp){
598    int err = 0; 
599    SAAlgorithm *algo = NULL;
600    int cipher_mode = CRYPTO_TFM_MODE_CBC;
601
602    dprintf("> sa=%p esp=%p\n", sa, esp);
603    dprintf("> cipher=%s\n", sa->cipher.name);
604    algo = sa_cipher_by_name(sa->cipher.name);
605    if(!algo){
606        wprintf("> Cipher unavailable: %s\n", sa->cipher.name);
607        err = -EINVAL;
608        goto exit;
609    }
610    esp->cipher.key_n = roundupto(sa->cipher.bits, 8);
611    // If cipher is null must use ECB because CBC algo does not support blocksize 1.
612    if(strcmp(sa->cipher.name, "cipher_null")){
613        cipher_mode = CRYPTO_TFM_MODE_ECB;
614    }
615    esp->cipher.tfm = crypto_alloc_tfm(sa->cipher.name, cipher_mode);
616    if(!esp->cipher.tfm){
617        err = -ENOMEM;
618        goto exit;
619    }
620    esp->cipher.block_n = roundupto(crypto_tfm_alg_blocksize(esp->cipher.tfm), 4);
621    esp->cipher.iv_n = crypto_tfm_alg_ivsize(esp->cipher.tfm);
622    esp->cipher.pad_n = 0;
623    if(esp->cipher.iv_n){
624        esp->cipher.iv = kmalloc(esp->cipher.iv_n, GFP_KERNEL);
625        get_random_bytes(esp->cipher.iv, esp->cipher.iv_n);
626    }
627    crypto_cipher_setkey(esp->cipher.tfm, esp->cipher.key, esp->cipher.key_n);
628    err = 0;
629  exit:
630    dprintf("< err=%d\n", err);
631    return err;
632}
633
634/** Initialize the digest for an ESP SA.
635 *
636 * @param sa ESP SA
637 * @param esp ESP state
638 * @return 0 on success, negative error code otherwise
639 */
640static int esp_digest_init(SAState *sa, ESPState *esp){
641    int err = 0;
642    SAAlgorithm *algo = NULL;
643   
644    dprintf(">\n");
645    esp->digest.key = sa->digest.key;
646    esp->digest.key_n = bits_to_bytes(roundupto(sa->digest.bits, 8));
647    esp->digest.tfm = crypto_alloc_tfm(sa->digest.name, 0);
648    if(!esp->digest.tfm){
649        err = -ENOMEM;
650        goto exit;
651    }
652    algo = sa_digest_by_name(sa->digest.name);
653    if(!algo){
654        wprintf("> Digest unavailable: %s\n", sa->digest.name);
655        err = -EINVAL;
656        goto exit;
657    }
658    esp->digest.icv = esp_hmac_digest;
659    esp->digest.icv_full_n = bits_to_bytes(algo->info.digest.icv_fullbits);
660    esp->digest.icv_n = bits_to_bytes(algo->info.digest.icv_truncbits);
661   
662    if(esp->digest.icv_full_n != crypto_tfm_alg_digestsize(esp->digest.tfm)){
663        err = -EINVAL;
664        wprintf("> digest %s, size %u != %hu\n",
665                sa->digest.name,
666                crypto_tfm_alg_digestsize(esp->digest.tfm),
667                esp->digest.icv_full_n);
668        goto exit;
669    }
670   
671    esp->digest.icv_tmp = kmalloc(esp->digest.icv_full_n, GFP_KERNEL);
672    if(!esp->digest.icv_tmp){
673        err = -ENOMEM;
674        goto exit;
675    }
676  exit:
677    dprintf("< err=%d\n", err);
678    return err;
679}
680
681/** Initialize an ESP SA.
682 *
683 * @param sa ESP SA
684 * @param args arguments
685 * @return 0 on success, negative error code otherwise
686 */
687static int esp_sa_init(SAState *sa, void *args){
688    int err = 0;
689    ESPState *esp = NULL;
690   
691    dprintf("> sa=%p\n", sa);
692    esp = kmalloc(sizeof(*esp), GFP_KERNEL);
693    if(!esp){
694        err = -ENOMEM;
695        goto exit;
696    }
697    *esp = (ESPState){};
698    err = esp_cipher_init(sa, esp);
699    if(err) goto exit;
700    err = esp_digest_init(sa, esp);
701    if(err) goto exit;
702    sa->data = esp;
703  exit:
704    if(err){
705        if(esp) esp_fini(esp);
706    }
707    dprintf("< err=%d\n", err);
708    return err;
709}
710
711/** SA type for ESP.
712 */
713static SAType esp_sa_type = {
714    .name     = "ESP",
715    .protocol = IPPROTO_ESP,
716    .init     = esp_sa_init,
717    .fini     = esp_sa_fini,
718    .size     = esp_sa_size,
719    .recv     = esp_sa_recv,
720    .send     = esp_sa_send
721};
722
723/** Get the ESP header from a packet.
724 *
725 * @param skb packet
726 * @param esph return parameter for header
727 * @return 0 on success, negative error code otherwise
728 */
729static int esp_skb_header(struct sk_buff *skb, ESPHdr **esph){
730    int err = 0;
731    if(skb->len < ESP_HDR_N){
732        err = -EINVAL;
733        goto exit;
734    }
735    *esph = (ESPHdr*)skb->data;
736  exit:
737    return err;
738}
739
740/** Handle an incoming skb with ESP protocol.
741 *
742 * Lookup spi, if state found hand to the state.
743 * If no state, check spi, if ok, create state and pass to it.
744 * If spi not ok, drop.
745 *
746 * Return value convention for protocols:
747 * >= 0 Protocol took the packet
748 * < 0  A -ve protocol id the packet should be re-received as.
749 *
750 * So always return >=0 if we took the packet, even if we dropped it.
751 *
752 * @param skb packet
753 * @return 0 on sucess, negative protocol number otherwise
754 */
755static int esp_protocol_recv(struct sk_buff *skb){
756    int err = 0;
757    const int eth_n = ETH_HLEN;
758    int ip_n;
759    ESPHdr *esph = NULL;
760    SAState *sa = NULL;
761    u32 addr;
762   
763    dprintf(">\n");
764#ifdef DEBUG
765    dprintf("> recv skb=\n"); 
766    skb_print_bits(skb, 0, skb->len);
767#endif
768    ip_n = (skb->nh.iph->ihl << 2);
769    if(skb->data == skb->mac.raw){
770        // skb->data points at ethernet header.
771        if (!pskb_may_pull(skb, eth_n + ip_n)){
772            wprintf("> Malformed skb\n");
773            err = -EINVAL;
774            goto exit;
775        }
776        skb_pull(skb, eth_n + ip_n);
777    }
778    addr = skb->nh.iph->daddr;
779    err = esp_skb_header(skb, &esph);
780    if(err) goto exit;
781    dprintf("> spi=%08x protocol=%d addr=" IPFMT "\n",
782            esph->spi, IPPROTO_ESP, NIPQUAD(addr));
783    sa = sa_table_lookup_spi(esph->spi, IPPROTO_ESP, addr);
784    if(!sa){
785        err = vnet_sa_create(esph->spi, IPPROTO_ESP, addr, &sa);
786        if(err) goto exit;
787    }
788    //todo: Return a -ve protocol instead? See esp_sa_recv.
789    err = SAState_recv(sa, skb);
790  exit:
791    if(sa) SAState_decref(sa);
792    if(err <= 0){
793        kfree_skb(skb);
794        err = 0;
795    }
796    dprintf("< err=%d\n", err);
797    return err;
798}
799
800/** Handle an ICMP error related to ESP.
801 *
802 * @param skb ICMP error packet
803 * @param info
804 */
805static void esp_protocol_icmp_err(struct sk_buff *skb, u32 info){
806    struct iphdr *iph = (struct iphdr*)skb->data;
807    ESPHdr *esph;
808    SAState *sa;
809   
810    dprintf("> ICMP error type=%d code=%d\n",
811            skb->h.icmph->type, skb->h.icmph->code);
812    if(skb->h.icmph->type != ICMP_DEST_UNREACH ||
813       skb->h.icmph->code != ICMP_FRAG_NEEDED){
814        return;
815    }
816   
817    //todo: need to check skb has enough len to do this.
818    esph = (ESPHdr*)(skb->data + (iph->ihl << 2));
819    sa = sa_table_lookup_spi(esph->spi, IPPROTO_ESP, iph->daddr);
820    if(!sa) return;
821    wprintf("> ICMP unreachable on SA ESP spi=%08x addr=" IPFMT "\n",
822            ntohl(esph->spi), NIPQUAD(iph->daddr));
823    SAState_decref(sa);
824}
825
826//============================================================================
827#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
828// Code for 2.6 kernel.
829
830/** Protocol handler for ESP.
831 */
832static struct net_protocol esp_protocol = {
833    .handler     = esp_protocol_recv,
834    .err_handler = esp_protocol_icmp_err
835};
836
837static int esp_protocol_add(void){
838    return inet_add_protocol(&esp_protocol, IPPROTO_ESP);
839}
840
841static int esp_protocol_del(void){
842    return inet_del_protocol(&esp_protocol, IPPROTO_ESP);
843}
844
845//============================================================================
846#else
847//============================================================================
848// Code for 2.4 kernel.
849
850/** Protocol handler for ESP.
851 */
852static struct inet_protocol esp_protocol = {
853    .name        = "ESP",
854    .protocol    = IPPROTO_ESP,
855    .handler     = esp_protocol_recv,
856    .err_handler = esp_protocol_icmp_err
857};
858
859static int esp_protocol_add(void){
860    inet_add_protocol(&esp_protocol);
861    return 0;
862}
863
864static int esp_protocol_del(void){
865    return inet_del_protocol(&esp_protocol);
866}
867
868#endif
869//============================================================================
870
871
872/** Initialize the ESP module.
873 * Registers the ESP protocol and SA type.
874 *
875 * @return 0 on success, negative error code otherwise
876 */
877int __init esp_module_init(void){
878    int err = 0;
879    dprintf(">\n");
880    err = SAType_add(&esp_sa_type);
881    if(err < 0){
882        eprintf("> Error adding esp sa type\n");
883        goto exit;
884    }
885    esp_protocol_add();
886  exit:
887    dprintf("< err=%d\n", err);
888    return err;
889}
890
891/** Finalize the ESP module.
892 * Deregisters the ESP protocol and SA type.
893 */
894void __exit esp_module_exit(void){
895    if(esp_protocol_del() < 0){
896        eprintf("> Error removing esp protocol\n");
897    }
898    if(SAType_del(&esp_sa_type) < 0){
899        eprintf("> Error removing esp sa type\n");
900    }
901}
902
903#endif // CONFIG_CRYPTO_HMAC
Note: See TracBrowser for help on using the repository browser.