static int ipcomp6_output(struct sk_buff **pskb) { int err; struct dst_entry *dst = (*pskb)->dst; struct xfrm_state *x = dst->xfrm; struct ipv6hdr *tmp_iph = NULL, *iph, *top_iph; int hdr_len = 0; struct ipv6_comp_hdr *ipch; struct ipcomp_data *ipcd = x->data; u8 *prevhdr; u8 nexthdr = 0; int plen, dlen; u8 *start, *scratch = ipcd->scratch; if ((*pskb)->ip_summed == CHECKSUM_HW) { err = skb_checksum_help(pskb, 0); if (err) goto error_nolock; } spin_lock_bh(&x->lock); err = xfrm_check_output(x, *pskb, AF_INET6); if (err) goto error; if (x->props.mode) { hdr_len = sizeof(struct ipv6hdr); nexthdr = IPPROTO_IPV6; iph = (*pskb)->nh.ipv6h; top_iph = (struct ipv6hdr *)skb_push(*pskb, sizeof(struct ipv6hdr)); top_iph->version = 6; top_iph->priority = iph->priority; top_iph->flow_lbl[0] = iph->flow_lbl[0]; top_iph->flow_lbl[1] = iph->flow_lbl[1]; top_iph->flow_lbl[2] = iph->flow_lbl[2]; top_iph->nexthdr = IPPROTO_IPV6; /* initial */ top_iph->payload_len = htons((*pskb)->len - sizeof(struct ipv6hdr)); top_iph->hop_limit = iph->hop_limit; memcpy(&top_iph->saddr, (struct in6_addr *)&x->props.saddr, sizeof(struct in6_addr)); memcpy(&top_iph->daddr, (struct in6_addr *)&x->id.daddr, sizeof(struct in6_addr)); (*pskb)->nh.raw = (*pskb)->data; /* == top_iph */ (*pskb)->h.raw = (*pskb)->nh.raw + hdr_len; } else { hdr_len = ip6_find_1stfragopt(*pskb, &prevhdr); nexthdr = *prevhdr; } /* check whether datagram len is larger than threshold */ if (((*pskb)->len - hdr_len) < ipcd->threshold) { goto out_ok; } if ((skb_is_nonlinear(*pskb) || skb_cloned(*pskb)) && skb_linearize(*pskb, GFP_ATOMIC) != 0) { err = -ENOMEM; goto error; } /* compression */ plen = (*pskb)->len - hdr_len; dlen = IPCOMP_SCRATCH_SIZE; start = (*pskb)->data + hdr_len; err = crypto_comp_compress(ipcd->tfm, start, plen, scratch, &dlen); if (err) { goto error; } if ((dlen + sizeof(struct ipv6_comp_hdr)) >= plen) { goto out_ok; } memcpy(start, scratch, dlen); pskb_trim(*pskb, hdr_len+dlen); /* insert ipcomp header and replace datagram */ tmp_iph = kmalloc(hdr_len, GFP_ATOMIC); if (!tmp_iph) { err = -ENOMEM; goto error; } memcpy(tmp_iph, (*pskb)->nh.raw, hdr_len); top_iph = (struct ipv6hdr*)skb_push(*pskb, sizeof(struct ipv6_comp_hdr)); memcpy(top_iph, tmp_iph, hdr_len); kfree(tmp_iph); if (x->props.mode && (x->props.flags & XFRM_STATE_NOECN)) IP6_ECN_clear(top_iph); top_iph->payload_len = htons((*pskb)->len - sizeof(struct ipv6hdr)); (*pskb)->nh.raw = (*pskb)->data; /* top_iph */ ip6_find_1stfragopt(*pskb, &prevhdr); *prevhdr = IPPROTO_COMP; ipch = (struct ipv6_comp_hdr *)((unsigned char *)top_iph + hdr_len); ipch->nexthdr = nexthdr; ipch->flags = 0; ipch->cpi = htons((u16 )ntohl(x->id.spi)); (*pskb)->h.raw = (unsigned char*)ipch; out_ok: x->curlft.bytes += (*pskb)->len; x->curlft.packets++; spin_unlock_bh(&x->lock); if (((*pskb)->dst = dst_pop(dst)) == NULL) { err = -EHOSTUNREACH; goto error_nolock; } err = NET_XMIT_BYPASS; out_exit: return err; error: spin_unlock_bh(&x->lock); error_nolock: kfree_skb(*pskb); goto out_exit; }
int esp6_output(struct sk_buff *skb) { int err; int hdr_len = 0; struct dst_entry *dst = skb->dst; struct xfrm_state *x = dst->xfrm; struct ipv6hdr *iph = NULL, *top_iph; struct ipv6_esp_hdr *esph; struct crypto_tfm *tfm; struct esp_data *esp; struct sk_buff *trailer; int blksize; int clen; int alen; int nfrags; u8 *prevhdr; u8 nexthdr = 0; /* First, if the skb is not checksummed, complete checksum. */ if (skb->ip_summed == CHECKSUM_HW && skb_checksum_help(skb) == NULL) { err = -EINVAL; goto error_nolock; } spin_lock_bh(&x->lock); err = xfrm_check_output(x, skb, AF_INET6); if (err) goto error; err = -ENOMEM; /* Strip IP header in transport mode. Save it. */ if (!x->props.mode) { hdr_len = ip6_find_1stfragopt(skb, &prevhdr); nexthdr = *prevhdr; *prevhdr = IPPROTO_ESP; iph = kmalloc(hdr_len, GFP_ATOMIC); if (!iph) { err = -ENOMEM; goto error; } memcpy(iph, skb->nh.raw, hdr_len); __skb_pull(skb, hdr_len); } /* Now skb is pure payload to encrypt */ /* Round to block size */ clen = skb->len; esp = x->data; alen = esp->auth.icv_trunc_len; tfm = esp->conf.tfm; blksize = (crypto_tfm_alg_blocksize(tfm) + 3) & ~3; clen = (clen + 2 + blksize-1)&~(blksize-1); if (esp->conf.padlen) clen = (clen + esp->conf.padlen-1)&~(esp->conf.padlen-1); if ((nfrags = skb_cow_data(skb, clen-skb->len+alen, &trailer)) < 0) { if (!x->props.mode && iph) kfree(iph); goto error; } /* Fill padding... */ do { int i; for (i=0; i<clen-skb->len - 2; i++) *(u8*)(trailer->tail + i) = i+1; } while (0); *(u8*)(trailer->tail + clen-skb->len - 2) = (clen - skb->len)-2; pskb_put(skb, trailer, clen - skb->len); if (x->props.mode) { iph = skb->nh.ipv6h; top_iph = (struct ipv6hdr*)skb_push(skb, x->props.header_len); esph = (struct ipv6_esp_hdr*)(top_iph+1); *(u8*)(trailer->tail - 1) = IPPROTO_IPV6; top_iph->version = 6; top_iph->priority = iph->priority; top_iph->flow_lbl[0] = iph->flow_lbl[0]; top_iph->flow_lbl[1] = iph->flow_lbl[1]; top_iph->flow_lbl[2] = iph->flow_lbl[2]; if (x->props.flags & XFRM_STATE_NOECN) IP6_ECN_clear(top_iph); top_iph->nexthdr = IPPROTO_ESP; top_iph->payload_len = htons(skb->len + alen - sizeof(struct ipv6hdr)); top_iph->hop_limit = iph->hop_limit; ipv6_addr_copy(&top_iph->saddr, (struct in6_addr *)&x->props.saddr); ipv6_addr_copy(&top_iph->daddr, (struct in6_addr *)&x->id.daddr); } else { esph = (struct ipv6_esp_hdr*)skb_push(skb, x->props.header_len); skb->h.raw = (unsigned char*)esph; top_iph = (struct ipv6hdr*)skb_push(skb, hdr_len); memcpy(top_iph, iph, hdr_len); kfree(iph); top_iph->payload_len = htons(skb->len + alen - sizeof(struct ipv6hdr)); *(u8*)(trailer->tail - 1) = nexthdr; } esph->spi = x->id.spi; esph->seq_no = htonl(++x->replay.oseq); if (esp->conf.ivlen) crypto_cipher_set_iv(tfm, esp->conf.ivec, crypto_tfm_alg_ivsize(tfm)); do { struct scatterlist sgbuf[nfrags>MAX_SG_ONSTACK ? 0 : nfrags]; struct scatterlist *sg = sgbuf; if (unlikely(nfrags > MAX_SG_ONSTACK)) { sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC); if (!sg) goto error; } skb_to_sgvec(skb, sg, esph->enc_data+esp->conf.ivlen-skb->data, clen); crypto_cipher_encrypt(tfm, sg, sg, clen); if (unlikely(sg != sgbuf)) kfree(sg); } while (0); if (esp->conf.ivlen) { memcpy(esph->enc_data, esp->conf.ivec, crypto_tfm_alg_ivsize(tfm)); crypto_cipher_get_iv(tfm, esp->conf.ivec, crypto_tfm_alg_ivsize(tfm)); } if (esp->auth.icv_full_len) { esp->auth.icv(esp, skb, (u8*)esph-skb->data, sizeof(struct ipv6_esp_hdr) + esp->conf.ivlen+clen, trailer->tail); pskb_put(skb, trailer, alen); } skb->nh.raw = skb->data; x->curlft.bytes += skb->len; x->curlft.packets++; spin_unlock_bh(&x->lock); if ((skb->dst = dst_pop(dst)) == NULL) { err = -EHOSTUNREACH; goto error_nolock; } return NET_XMIT_BYPASS; error: spin_unlock_bh(&x->lock); error_nolock: kfree_skb(skb); return err; }