esp.c

来自「xen虚拟机源代码安装包」· C语言 代码 · 共 904 行 · 第 1/2 页

    //todo Maybe not safe to do this anymore.
    memmove(skb->mac.raw + head_n, skb->mac.raw, (skb->data - skb->mac.raw));
    skb->mac.raw += head_n;
    skb->nh.raw  += head_n;
    // Move skb->data back to ethernet header.
    // Do in 2 moves to ensure offsets are +ve,
    // since args to skb_pull/skb_push are unsigned.
    skb_pull_vn(skb, head_n);
    __skb_push(skb, skb->data - skb->mac.raw);
    // After this esph is invalid.
    esph = NULL;
    // Trim padding, restore protocol in IP header.
    pad = skb_trim_tail(skb, ESP_PAD_N);
    text_n -= ESP_PAD_N;
    if((pad->pad_n > 255) | (pad->pad_n > text_n)){
        wprintf("> Invalid padding: pad_n=%d text_n=%d\n", pad->pad_n, text_n);
        goto exit;
    }
    skb_trim_tail(skb, pad->pad_n);
    skb->nh.iph->protocol = pad->protocol;
    err = skb_push_context(skb, vnet, sa->ident.addr, IPPROTO_ESP,
                           sa, esp_context_free_fn);
    if(err) goto exit;
    // Increase sa refcount now the skb context refers to it.
    // Refcount is decreased by esp_context_free_fn.
    SAState_incref(sa);
    // Deliver skb to be received by network code.
    // Not safe to refer to the skb after this.
    // todo: return -skb->nh.iph->protocol instead?
    netif_rx(skb);
  exit:
    if(mine){
        if(err < 0){
            kfree_skb(skb);
        }
        err = 1;
    }
    dprintf("< skb=%p err=%d\n", skb, err);
    return err;
}

/** Estimate the packet size for some data using ESP processing.    
 *
 * @param sa ESP SA
 * @param data_n data size
 * @return size after ESP processing
 */
static u32 esp_sa_size(SAState *sa, int data_n){
    // Even in transport mode have to round up to blocksize.
    // Have to add some padding for alignment even if pad_n is zero.
    ESPState *esp = sa->data;
    
    data_n = roundupto(data_n + ESP_PAD_N, esp->cipher.block_n);
    if(esp->cipher.pad_n > 0){
        data_n = roundupto(data_n, esp->cipher.pad_n);
    }
    data_n += esp->digest.icv_n;
    //data_n += esp->cipher.iv_n;
    data_n += ESP_HDR_N;
    return data_n;
}

/** Compute an icv using HMAC digest.
 *
 * @param esp ESP state
 * @param skb packet to digest
 * @param offset offset to start at
 * @param len number of bytes to digest
 * @param icv return parameter for ICV
 * @return 0 on success, negative error code otherwise
 */
static inline void esp_hmac_digest(ESPState *esp, struct sk_buff *skb,
                                   int offset, int len, u8 *icv){
    int err = 0;
    struct crypto_tfm *digest = esp->digest.tfm;
    char *icv_tmp = esp->digest.icv_tmp;
    int sg_n = skb_shinfo(skb)->nr_frags + 1;
    struct scatterlist sg[sg_n];

    dprintf("> offset=%d len=%d\n", offset, len);
    memset(icv, 0, esp->digest.icv_n);
    if(DEBUG_ICV){
        dprintf("> key len=%d\n", esp->digest.key_n);
        printk("\nkey=");
        buf_print(esp->digest.key,esp->digest.key_n); 
    }
    crypto_hmac_init(digest, esp->digest.key, &esp->digest.key_n);
    err = skb_scatterlist(skb, sg, &sg_n, offset, len);
    crypto_hmac_update(digest, sg, sg_n);
    crypto_hmac_final(digest, esp->digest.key, &esp->digest.key_n, icv_tmp);
    if(DEBUG_ICV){
        dprintf("> digest len=%d ", esp->digest.icv_n);
        printk("\nval=");
        buf_print(icv_tmp, esp->digest.icv_n);
    }
    memcpy(icv, icv_tmp, esp->digest.icv_n);
    dprintf("<\n");
}

/** Finish up an esp state.
 * Releases the digest, cipher, iv and frees the state.
 *
 * @parma esp state
 */
static void esp_fini(ESPState *esp){
    if(!esp) return;
    if(esp->digest.tfm){
        crypto_free_tfm(esp->digest.tfm);
        esp->digest.tfm = NULL; 
    }
    if(esp->digest.icv_tmp){
        kfree(esp->digest.icv_tmp);
        esp->digest.icv_tmp = NULL;
    }
    if(esp->cipher.tfm){
        crypto_free_tfm(esp->cipher.tfm);
        esp->cipher.tfm = NULL;
    }
    if(esp->cipher.iv){
        kfree(esp->cipher.iv);
        esp->cipher.iv = NULL;
    }
    kfree(esp);
}

/** Release an ESP SA.
 *
 * @param sa ESO SA
 */
static void esp_sa_fini(SAState *sa){
    ESPState *esp;
    if(!sa) return;
    esp = sa->data;
    if(!esp) return;
    esp_fini(esp);
    sa->data = NULL;
}

/** Initialize the cipher for an ESP SA.
 *
 * @param sa ESP SA
 * @param esp ESP state
 * @return 0 on success, negative error code otherwise
 */
static int esp_cipher_init(SAState *sa, ESPState *esp){
    int err = 0; 
    SAAlgorithm *algo = NULL;
    int cipher_mode = CRYPTO_TFM_MODE_CBC;

    dprintf("> sa=%p esp=%p\n", sa, esp);
    dprintf("> cipher=%s\n", sa->cipher.name);
    algo = sa_cipher_by_name(sa->cipher.name);
    if(!algo){
        wprintf("> Cipher unavailable: %s\n", sa->cipher.name);
        err = -EINVAL;
        goto exit;
    }
    esp->cipher.key_n = roundupto(sa->cipher.bits, 8);
    // If cipher is null must use ECB because CBC algo does not support blocksize 1.
    if(strcmp(sa->cipher.name, "cipher_null")){
        cipher_mode = CRYPTO_TFM_MODE_ECB;
    }
    esp->cipher.tfm = crypto_alloc_tfm(sa->cipher.name, cipher_mode);
    if(!esp->cipher.tfm){
        err = -ENOMEM;
        goto exit;
    }
    esp->cipher.block_n = roundupto(crypto_tfm_alg_blocksize(esp->cipher.tfm), 4);
    esp->cipher.iv_n = crypto_tfm_alg_ivsize(esp->cipher.tfm);
    esp->cipher.pad_n = 0;
    if(esp->cipher.iv_n){
        esp->cipher.iv = kmalloc(esp->cipher.iv_n, GFP_KERNEL);
        get_random_bytes(esp->cipher.iv, esp->cipher.iv_n);
    }
    crypto_cipher_setkey(esp->cipher.tfm, esp->cipher.key, esp->cipher.key_n);
    err = 0;
  exit:
    dprintf("< err=%d\n", err);
    return err;
}

/** Initialize the digest for an ESP SA.
 *
 * @param sa ESP SA
 * @param esp ESP state
 * @return 0 on success, negative error code otherwise
 */
static int esp_digest_init(SAState *sa, ESPState *esp){
    int err = 0;
    SAAlgorithm *algo = NULL;
    
    dprintf(">\n");
    esp->digest.key = sa->digest.key;
    esp->digest.key_n = bits_to_bytes(roundupto(sa->digest.bits, 8));
    esp->digest.tfm = crypto_alloc_tfm(sa->digest.name, 0);
    if(!esp->digest.tfm){
        err = -ENOMEM;
        goto exit;
    }
    algo = sa_digest_by_name(sa->digest.name);
    if(!algo){
        wprintf("> Digest unavailable: %s\n", sa->digest.name);
        err = -EINVAL;
        goto exit;
    }
    esp->digest.icv = esp_hmac_digest;
    esp->digest.icv_full_n = bits_to_bytes(algo->info.digest.icv_fullbits);
    esp->digest.icv_n = bits_to_bytes(algo->info.digest.icv_truncbits);
    
    if(esp->digest.icv_full_n != crypto_tfm_alg_digestsize(esp->digest.tfm)){
        err = -EINVAL;
        wprintf("> digest %s, size %u != %hu\n",
                sa->digest.name,
                crypto_tfm_alg_digestsize(esp->digest.tfm),
                esp->digest.icv_full_n);
        goto exit;
    }
    
    esp->digest.icv_tmp = kmalloc(esp->digest.icv_full_n, GFP_KERNEL);
    if(!esp->digest.icv_tmp){
        err = -ENOMEM;
        goto exit;
    }
  exit:
    dprintf("< err=%d\n", err);
    return err;
}

/** Initialize an ESP SA.
 *
 * @param sa ESP SA
 * @param args arguments
 * @return 0 on success, negative error code otherwise
 */
static int esp_sa_init(SAState *sa, void *args){
    int err = 0;
    ESPState *esp = NULL;
    
    dprintf("> sa=%p\n", sa);
    esp = kmalloc(sizeof(*esp), GFP_KERNEL);
    if(!esp){
        err = -ENOMEM;
        goto exit;
    }
    *esp = (ESPState){};
    err = esp_cipher_init(sa, esp);
    if(err) goto exit;
    err = esp_digest_init(sa, esp);
    if(err) goto exit;
    sa->data = esp;
  exit:
    if(err){
        if(esp) esp_fini(esp);
    }
    dprintf("< err=%d\n", err);
    return err;
}

/** SA type for ESP.
 */
static SAType esp_sa_type = {
    .name     = "ESP",
    .protocol = IPPROTO_ESP,
    .init     = esp_sa_init,
    .fini     = esp_sa_fini,
    .size     = esp_sa_size,
    .recv     = esp_sa_recv,
    .send     = esp_sa_send
};

/** Get the ESP header from a packet.
 *
 * @param skb packet
 * @param esph return parameter for header
 * @return 0 on success, negative error code otherwise
 */
static int esp_skb_header(struct sk_buff *skb, ESPHdr **esph){
    int err = 0;
    if(skb->len < ESP_HDR_N){
        err = -EINVAL;
        goto exit;
    }
    *esph = (ESPHdr*)skb->data;
  exit:
    return err;
}

/** Handle an incoming skb with ESP protocol.
 *
 * Lookup spi, if state found hand to the state.
 * If no state, check spi, if ok, create state and pass to it.
 * If spi not ok, drop.
 *
 * Return value convention for protocols:
 * >= 0 Protocol took the packet
 * < 0  A -ve protocol id the packet should be re-received as.
 *
 * So always return >=0 if we took the packet, even if we dropped it.
 * 
 * @param skb packet
 * @return 0 on sucess, negative protocol number otherwise
 */
static int esp_protocol_recv(struct sk_buff *skb){
    int err = 0;
    const int eth_n = ETH_HLEN;
    int ip_n;
    ESPHdr *esph = NULL;
    SAState *sa = NULL;
    u32 addr;
    
    dprintf(">\n");
#ifdef DEBUG
    dprintf("> recv skb=\n"); 
    skb_print_bits("", skb, 0, skb->len);
#endif
    ip_n = (skb->nh.iph->ihl << 2);
    if(skb->data == skb->mac.raw){
        // skb->data points at ethernet header.
        if (!pskb_may_pull(skb, eth_n + ip_n)){
            wprintf("> Malformed skb\n");
            err = -EINVAL;
            goto exit;
        }
        skb_pull_vn(skb, eth_n + ip_n);
    }
    addr = skb->nh.iph->daddr;
    err = esp_skb_header(skb, &esph);
    if(err) goto exit;
    dprintf("> spi=%08x protocol=%d addr=" IPFMT "\n",
            esph->spi, IPPROTO_ESP, NIPQUAD(addr));
    sa = sa_table_lookup_spi(esph->spi, IPPROTO_ESP, addr);
    if(!sa){
        err = vnet_sa_create(esph->spi, IPPROTO_ESP, addr, &sa);
        if(err) goto exit;
    }
    //todo: Return a -ve protocol instead? See esp_sa_recv.
    err = SAState_recv(sa, skb);
  exit:
    if(sa) SAState_decref(sa);
    if(err <= 0){
        kfree_skb(skb);
        err = 0;
    }
    dprintf("< err=%d\n", err);
    return err;
}

/** Handle an ICMP error related to ESP.
 *
 * @param skb ICMP error packet
 * @param info
 */
static void esp_protocol_icmp_err(struct sk_buff *skb, u32 info){
    struct iphdr *iph = (struct iphdr*)skb->data;
    ESPHdr *esph;
    SAState *sa;
    
    dprintf("> ICMP error type=%d code=%d\n",
            skb->h.icmph->type, skb->h.icmph->code);
    if(skb->h.icmph->type != ICMP_DEST_UNREACH ||
       skb->h.icmph->code != ICMP_FRAG_NEEDED){
        return;
    }
    
    //todo: need to check skb has enough len to do this.
    esph = (ESPHdr*)(skb->data + (iph->ihl << 2));
    sa = sa_table_lookup_spi(esph->spi, IPPROTO_ESP, iph->daddr);
    if(!sa) return;
    wprintf("> ICMP unreachable on SA ESP spi=%08x addr=" IPFMT "\n",
            ntohl(esph->spi), NIPQUAD(iph->daddr));
    SAState_decref(sa);
}

//============================================================================
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
// Code for 2.6 kernel.

/** Protocol handler for ESP.
 */
static struct net_protocol esp_protocol = {
    .handler     = esp_protocol_recv,
    .err_handler = esp_protocol_icmp_err
};

static int esp_protocol_add(void){
    return inet_add_protocol(&esp_protocol, IPPROTO_ESP);
}

static int esp_protocol_del(void){
    return inet_del_protocol(&esp_protocol, IPPROTO_ESP);
}

//============================================================================
#else
//============================================================================
// Code for 2.4 kernel.

/** Protocol handler for ESP.
 */
static struct inet_protocol esp_protocol = {
    .name        = "ESP",
    .protocol    = IPPROTO_ESP,
    .handler     = esp_protocol_recv,
    .err_handler = esp_protocol_icmp_err
};

static int esp_protocol_add(void){
    inet_add_protocol(&esp_protocol);
    return 0;
}

static int esp_protocol_del(void){
    return inet_del_protocol(&esp_protocol);
}

#endif
//============================================================================


/** Initialize the ESP module.
 * Registers the ESP protocol and SA type.
 *
 * @return 0 on success, negative error code otherwise
 */
int __init esp_module_init(void){
    int err = 0;
    dprintf(">\n");
    err = SAType_add(&esp_sa_type);
    if(err < 0){
        eprintf("> Error adding esp sa type\n");
        goto exit;
    }
    esp_protocol_add();
  exit:
    dprintf("< err=%d\n", err);
    return err;
}

/** Finalize the ESP module.
 * Deregisters the ESP protocol and SA type.
 */
void __exit esp_module_exit(void){
    if(esp_protocol_del() < 0){
        eprintf("> Error removing esp protocol\n");
    }
    if(SAType_del(&esp_sa_type) < 0){
        eprintf("> Error removing esp sa type\n");
    }
}

#endif // CONFIG_CRYPTO_HMAC