static int ipcomp_compress(struct xfrm_state *x, struct sk_buff *skb)
{
int err, plen, dlen, ihlen;
struct iphdr *iph = skb->nh.iph;
struct ipcomp_data *ipcd = x->data;
u8 *start, *scratch = ipcd->scratch;
ihlen = iph->ihl * 4;
plen = skb->len - ihlen;
dlen = IPCOMP_SCRATCH_SIZE;
start = skb->data + ihlen;
err = crypto_comp_compress(ipcd->tfm, start, plen, scratch, &dlen);
if (err)
goto out;
if ((dlen + sizeof(struct ip_comp_hdr)) >= plen) {
err = -EMSGSIZE;
goto out;
}
memcpy(start, scratch, dlen);
pskb_trim(skb, ihlen + dlen);
out:
return err;
}
static int ipcomp_compress(struct xfrm_state *x, struct sk_buff *skb)
{
struct ipcomp_data *ipcd = x->data;
const int plen = skb->len;
int dlen = IPCOMP_SCRATCH_SIZE;
u8 *start = skb->data;
const int cpu = get_cpu();
u8 *scratch = *per_cpu_ptr(ipcomp_scratches, cpu);
struct crypto_comp *tfm = *per_cpu_ptr(ipcd->tfms, cpu);
int err;
local_bh_disable();
err = crypto_comp_compress(tfm, start, plen, scratch, &dlen);
local_bh_enable();
if (err)
goto out;
if ((dlen + sizeof(struct ip_comp_hdr)) >= plen) {
err = -EMSGSIZE;
goto out;
}
memcpy(start + sizeof(struct ip_comp_hdr), scratch, dlen);
put_cpu();
pskb_trim(skb, dlen + sizeof(struct ip_comp_hdr));
return 0;
out:
put_cpu();
return err;
}
/**
* sk_filter - run a packet through a socket filter
* @sk: sock associated with &sk_buff
* @skb: buffer to filter
*
* Run the filter code and then cut skb->data to correct size returned by
* sk_run_filter. If pkt_len is 0 we toss packet. If skb->len is smaller
* than pkt_len we keep whole skb->data. This is the socket level
* wrapper to sk_run_filter. It returns 0 if the packet should
* be accepted or -EPERM if the packet should be tossed.
*
*/
int sk_filter(struct sock *sk, struct sk_buff *skb)
{
int err;
struct sk_filter *filter;
/*
* If the skb was allocated from pfmemalloc reserves, only
* allow SOCK_MEMALLOC sockets to use it as this socket is
* helping free memory
*/
if (skb_pfmemalloc(skb) && !sock_flag(sk, SOCK_MEMALLOC))
return -ENOMEM;
err = security_sock_rcv_skb(sk, skb);
if (err)
return err;
rcu_read_lock();
filter = rcu_dereference(sk->sk_filter);
if (filter) {
unsigned int pkt_len = SK_RUN_FILTER(filter, skb);
err = pkt_len ? pskb_trim(skb, pkt_len) : -EPERM;
}
rcu_read_unlock();
return err;
}
static int ipcomp6_output(struct sk_buff **pskb)
{
int err;
struct dst_entry *dst = (*pskb)->dst;
struct xfrm_state *x = dst->xfrm;
struct ipv6hdr *top_iph;
int hdr_len;
struct ipv6_comp_hdr *ipch;
struct ipcomp_data *ipcd = x->data;
int plen, dlen;
u8 *start, *scratch = ipcd->scratch;
hdr_len = (*pskb)->h.raw - (*pskb)->data;
/* 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)->h.raw;
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 + sizeof(struct ip_comp_hdr), scratch, dlen);
pskb_trim(*pskb, hdr_len + dlen + sizeof(struct ip_comp_hdr));
/* insert ipcomp header and replace datagram */
top_iph = (struct ipv6hdr *)(*pskb)->data;
top_iph->payload_len = htons((*pskb)->len - sizeof(struct ipv6hdr));
ipch = (struct ipv6_comp_hdr *)start;
ipch->nexthdr = *(*pskb)->nh.raw;
ipch->flags = 0;
ipch->cpi = htons((u16 )ntohl(x->id.spi));
*(*pskb)->nh.raw = IPPROTO_COMP;
out_ok:
err = 0;
error:
return err;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
struct sock *sk = pn_find_sock_by_sa(net, &sa);
if (sk)
return sk_receive_skb(sk, skb, 0);
else {
// patch for VT call, do not response for un-opened socket resource
goto out;
// can never get this line
}
#if 0 // do not make auto response
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
#endif
}
out:
kfree_skb(skb);
return NET_RX_DROP;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct phonethdr *ph;
struct sock *sk;
struct sockaddr_pn sa;
u16 len;
if (dev_net(dev) != &init_net)
goto out;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
if (pn_sockaddr_get_addr(&sa) == 0)
goto out; /* currently, we cannot be device 0 */
sk = pn_find_sock_by_sa(&sa);
if (sk == NULL) {
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
goto out;
}
/* Push data to the socket (or other sockets connected to it). */
return sk_receive_skb(sk, skb, 0);
out:
kfree_skb(skb);
return NET_RX_DROP;
}
/**
* sk_filter - run a packet through a socket filter
* @sk: sock associated with &sk_buff
* @skb: buffer to filter
*
* Run the filter code and then cut skb->data to correct size returned by
* sk_run_filter. If pkt_len is 0 we toss packet. If skb->len is smaller
* than pkt_len we keep whole skb->data. This is the socket level
* wrapper to sk_run_filter. It returns 0 if the packet should
* be accepted or -EPERM if the packet should be tossed.
*
*/
int sk_filter(struct sock *sk, struct sk_buff *skb)
{
int err;
struct sk_filter *filter;
err = security_sock_rcv_skb(sk, skb);
if (err)
return err;
rcu_read_lock_bh();
filter = rcu_dereference_bh(sk->sk_filter);
if (filter) {
unsigned int pkt_len = SK_RUN_FILTER(filter, skb);
err = pkt_len ? pskb_trim(skb, pkt_len) : -EPERM;
}
rcu_read_unlock_bh();
return err;
}
/**
* sk_filter - run a packet through a socket filter
* @sk: sock associated with &sk_buff
* @skb: buffer to filter
*
* Run the filter code and then cut skb->data to correct size returned by
* sk_run_filter. If pkt_len is 0 we toss packet. If skb->len is smaller
* than pkt_len we keep whole skb->data. This is the socket level
* wrapper to sk_run_filter. It returns 0 if the packet should
* be accepted or -EPERM if the packet should be tossed.
*
*/
int sk_filter(struct sock *sk, struct sk_buff *skb)
{
int err;
struct sk_filter *filter;
err = security_sock_rcv_skb(sk, skb);
if (err)
return err;
err = provenance_sock_rcv_skb(sk, skb);
if (err)
return err;
rcu_read_lock_bh();
filter = rcu_dereference(sk->sk_filter);
if (filter) {
unsigned int pkt_len = sk_run_filter(skb, filter->insns,
filter->len);
err = pkt_len ? pskb_trim(skb, pkt_len) : -EPERM;
}
rcu_read_unlock_bh();
return err;
}
static int esp6_input(struct xfrm_state *x, struct sk_buff *skb)
{
struct ipv6hdr *iph;
struct ipv6_esp_hdr *esph;
struct esp_data *esp = x->data;
struct crypto_blkcipher *tfm = esp->conf.tfm;
struct blkcipher_desc desc = { .tfm = tfm };
struct sk_buff *trailer;
int blksize = ALIGN(crypto_blkcipher_blocksize(tfm), 4);
int alen = esp->auth.icv_trunc_len;
int elen = skb->len - sizeof(struct ipv6_esp_hdr) - esp->conf.ivlen - alen;
int hdr_len = skb->h.raw - skb->nh.raw;
int nfrags;
int ret = 0;
if (!pskb_may_pull(skb, sizeof(struct ipv6_esp_hdr))) {
ret = -EINVAL;
goto out;
}
if (elen <= 0 || (elen & (blksize-1))) {
ret = -EINVAL;
goto out;
}
/* If integrity check is required, do this. */
if (esp->auth.icv_full_len) {
u8 sum[alen];
ret = esp_mac_digest(esp, skb, 0, skb->len - alen);
if (ret)
goto out;
if (skb_copy_bits(skb, skb->len - alen, sum, alen))
BUG();
if (unlikely(memcmp(esp->auth.work_icv, sum, alen))) {
x->stats.integrity_failed++;
ret = -EINVAL;
goto out;
}
}
if ((nfrags = skb_cow_data(skb, 0, &trailer)) < 0) {
ret = -EINVAL;
goto out;
}
skb->ip_summed = CHECKSUM_NONE;
esph = (struct ipv6_esp_hdr*)skb->data;
iph = skb->nh.ipv6h;
/* Get ivec. This can be wrong, check against another impls. */
if (esp->conf.ivlen)
crypto_blkcipher_set_iv(tfm, esph->enc_data, esp->conf.ivlen);
{
u8 nexthdr[2];
struct scatterlist *sg = &esp->sgbuf[0];
u8 padlen;
if (unlikely(nfrags > ESP_NUM_FAST_SG)) {
sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC);
if (!sg) {
ret = -ENOMEM;
goto out;
}
}
skb_to_sgvec(skb, sg, sizeof(struct ipv6_esp_hdr) + esp->conf.ivlen, elen);
ret = crypto_blkcipher_decrypt(&desc, sg, sg, elen);
if (unlikely(sg != &esp->sgbuf[0]))
kfree(sg);
if (unlikely(ret))
goto out;
if (skb_copy_bits(skb, skb->len-alen-2, nexthdr, 2))
BUG();
padlen = nexthdr[0];
if (padlen+2 >= elen) {
LIMIT_NETDEBUG(KERN_WARNING "ipsec esp packet is garbage padlen=%d, elen=%d\n", padlen+2, elen);
ret = -EINVAL;
goto out;
}
/* ... check padding bits here. Silly. :-) */
pskb_trim(skb, skb->len - alen - padlen - 2);
ret = nexthdr[1];
}
skb->h.raw = __skb_pull(skb, sizeof(*esph) + esp->conf.ivlen) - hdr_len;
out:
return ret;
}
inline __attribute__((noinline)) int ___pskb_trim(void)
{
pskb_trim();
return 0;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
/* check if this is broadcasted */
if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
struct sock *sk = pn_find_sock_by_sa(net, &sa);
if (sk)
return sk_receive_skb(sk, skb, 0);
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
} else if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
goto out; /* Race between address deletion and loopback */
else {
/* Phonet packet routing */
struct net_device *out_dev;
out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa));
if (!out_dev) {
LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n",
pn_sockaddr_get_addr(&sa));
goto out;
}
__skb_push(skb, sizeof(struct phonethdr));
skb->dev = out_dev;
if (out_dev == dev) {
LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n",
pn_sockaddr_get_addr(&sa), dev->name);
goto out_dev;
}
/* Some drivers (e.g. TUN) do not allocate HW header space */
if (skb_cow_head(skb, out_dev->hard_header_len))
goto out_dev;
if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL,
skb->len) < 0)
goto out_dev;
dev_queue_xmit(skb);
dev_put(out_dev);
return NET_RX_SUCCESS;
out_dev:
dev_put(out_dev);
}
out:
kfree_skb(skb);
return NET_RX_DROP;
}
static int packet_rcv(struct sk_buff *skb, struct net_device *dev, struct packet_type *pt)
{
struct sock *sk;
struct sockaddr_ll *sll;
struct packet_sock *po;
u8 * skb_head = skb->data;
int skb_len = skb->len;
unsigned snaplen;
if (skb->pkt_type == PACKET_LOOPBACK)
goto drop;
sk = pt->af_packet_priv;
po = pkt_sk(sk);
skb->dev = dev;
if (dev->hard_header) {
/* The device has an explicit notion of ll header,
exported to higher levels.
Otherwise, the device hides datails of it frame
structure, so that corresponding packet head
never delivered to user.
*/
if (sk->sk_type != SOCK_DGRAM)
skb_push(skb, skb->data - skb->mac.raw);
else if (skb->pkt_type == PACKET_OUTGOING) {
/* Special case: outgoing packets have ll header at head */
skb_pull(skb, skb->nh.raw - skb->data);
}
}
snaplen = skb->len;
if (sk->sk_filter) {
unsigned res = run_filter(skb, sk, snaplen);
if (res == 0)
goto drop_n_restore;
if (snaplen > res)
snaplen = res;
}
if (atomic_read(&sk->sk_rmem_alloc) + skb->truesize >=
(unsigned)sk->sk_rcvbuf)
goto drop_n_acct;
if (skb_shared(skb)) {
struct sk_buff *nskb = skb_clone(skb, GFP_ATOMIC);
if (nskb == NULL)
goto drop_n_acct;
if (skb_head != skb->data) {
skb->data = skb_head;
skb->len = skb_len;
}
kfree_skb(skb);
skb = nskb;
}
sll = (struct sockaddr_ll*)skb->cb;
sll->sll_family = AF_PACKET;
sll->sll_hatype = dev->type;
sll->sll_protocol = skb->protocol;
sll->sll_pkttype = skb->pkt_type;
sll->sll_ifindex = dev->ifindex;
sll->sll_halen = 0;
if (dev->hard_header_parse)
sll->sll_halen = dev->hard_header_parse(skb, sll->sll_addr);
if (pskb_trim(skb, snaplen))
goto drop_n_acct;
skb_set_owner_r(skb, sk);
skb->dev = NULL;
dst_release(skb->dst);
skb->dst = NULL;
spin_lock(&sk->sk_receive_queue.lock);
po->stats.tp_packets++;
__skb_queue_tail(&sk->sk_receive_queue, skb);
spin_unlock(&sk->sk_receive_queue.lock);
sk->sk_data_ready(sk, skb->len);
return 0;
drop_n_acct:
spin_lock(&sk->sk_receive_queue.lock);
po->stats.tp_drops++;
spin_unlock(&sk->sk_receive_queue.lock);
drop_n_restore:
if (skb_head != skb->data && skb_shared(skb)) {
skb->data = skb_head;
skb->len = skb_len;
}
drop:
kfree_skb(skb);
return 0;
}
static void ip6_frag_queue(struct frag_queue *fq, struct sk_buff *skb,
struct frag_hdr *fhdr, int nhoff)
{
struct sk_buff *prev, *next;
int offset, end;
if (fq->last_in & COMPLETE)
goto err;
offset = ntohs(fhdr->frag_off) & ~0x7;
end = offset + (ntohs(skb->nh.ipv6h->payload_len) -
((u8 *) (fhdr + 1) - (u8 *) (skb->nh.ipv6h + 1)));
if ((unsigned int)end >= 65536) {
icmpv6_param_prob(skb,ICMPV6_HDR_FIELD, (u8*)&fhdr->frag_off - skb->nh.raw);
return;
}
if (skb->ip_summed == CHECKSUM_HW)
skb->csum = csum_sub(skb->csum,
csum_partial(skb->nh.raw, (u8*)(fhdr+1)-skb->nh.raw, 0));
/* Is this the final fragment? */
if (!(fhdr->frag_off & __constant_htons(0x0001))) {
/* If we already have some bits beyond end
* or have different end, the segment is corrupted.
*/
if (end < fq->len ||
((fq->last_in & LAST_IN) && end != fq->len))
goto err;
fq->last_in |= LAST_IN;
fq->len = end;
} else {
/* Check if the fragment is rounded to 8 bytes.
* Required by the RFC.
*/
if (end & 0x7) {
/* RFC2460 says always send parameter problem in
* this case. -DaveM
*/
icmpv6_param_prob(skb, ICMPV6_HDR_FIELD,
offsetof(struct ipv6hdr, payload_len));
return;
}
if (end > fq->len) {
/* Some bits beyond end -> corruption. */
if (fq->last_in & LAST_IN)
goto err;
fq->len = end;
}
}
if (end == offset)
goto err;
/* Point into the IP datagram 'data' part. */
if (!pskb_pull(skb, (u8 *) (fhdr + 1) - skb->data))
goto err;
if (end-offset < skb->len) {
if (pskb_trim(skb, end - offset))
goto err;
if (skb->ip_summed != CHECKSUM_UNNECESSARY)
skb->ip_summed = CHECKSUM_NONE;
}
/* Find out which fragments are in front and at the back of us
* in the chain of fragments so far. We must know where to put
* this fragment, right?
*/
prev = NULL;
for(next = fq->fragments; next != NULL; next = next->next) {
if (FRAG6_CB(next)->offset >= offset)
break; /* bingo! */
prev = next;
}
/* We found where to put this one. Check for overlap with
* preceding fragment, and, if needed, align things so that
* any overlaps are eliminated.
*/
if (prev) {
int i = (FRAG6_CB(prev)->offset + prev->len) - offset;
if (i > 0) {
offset += i;
if (end <= offset)
goto err;
if (!pskb_pull(skb, i))
goto err;
if (skb->ip_summed != CHECKSUM_UNNECESSARY)
skb->ip_summed = CHECKSUM_NONE;
}
}
/* Look for overlap with succeeding segments.
* If we can merge fragments, do it.
*/
while (next && FRAG6_CB(next)->offset < end) {
int i = end - FRAG6_CB(next)->offset; /* overlap is 'i' bytes */
if (i < next->len) {
/* Eat head of the next overlapped fragment
* and leave the loop. The next ones cannot overlap.
*/
if (!pskb_pull(next, i))
goto err;
FRAG6_CB(next)->offset += i; /* next fragment */
fq->meat -= i;
if (next->ip_summed != CHECKSUM_UNNECESSARY)
next->ip_summed = CHECKSUM_NONE;
break;
} else {
struct sk_buff *free_it = next;
/* Old fragmnet is completely overridden with
* new one drop it.
*/
next = next->next;
if (prev)
prev->next = next;
else
fq->fragments = next;
fq->meat -= free_it->len;
frag_kfree_skb(free_it);
}
}
FRAG6_CB(skb)->offset = offset;
/* Insert this fragment in the chain of fragments. */
skb->next = next;
if (prev)
prev->next = skb;
else
fq->fragments = skb;
if (skb->dev)
fq->iif = skb->dev->ifindex;
skb->dev = NULL;
fq->stamp = skb->stamp;
fq->meat += skb->len;
atomic_add(skb->truesize, &ip6_frag_mem);
/* The first fragment.
* nhoffset is obtained from the first fragment, of course.
*/
if (offset == 0) {
fq->nhoffset = nhoff;
fq->last_in |= FIRST_IN;
}
return;
err:
kfree_skb(skb);
}
/*
* Note: detecting truncated vs. non-truncated authentication data is very
* expensive, so we only support truncated data, which is the recommended
* and common case.
*/
static int esp_input(struct xfrm_state *x, struct sk_buff *skb)
{
struct iphdr *iph;
struct ip_esp_hdr *esph;
struct esp_data *esp = x->data;
struct crypto_blkcipher *tfm = esp->conf.tfm;
struct blkcipher_desc desc = { .tfm = tfm };
struct sk_buff *trailer;
int blksize = ALIGN(crypto_blkcipher_blocksize(tfm), 4);
int alen = esp->auth.icv_trunc_len;
int elen = skb->len - sizeof(struct ip_esp_hdr) - esp->conf.ivlen - alen;
int nfrags;
int ihl;
u8 nexthdr[2];
struct scatterlist *sg;
int padlen;
int err;
if (!pskb_may_pull(skb, sizeof(struct ip_esp_hdr)))
goto out;
if (elen <= 0 || (elen & (blksize-1)))
goto out;
/* If integrity check is required, do this. */
if (esp->auth.icv_full_len) {
u8 sum[alen];
err = esp_mac_digest(esp, skb, 0, skb->len - alen);
if (err)
goto out;
if (skb_copy_bits(skb, skb->len - alen, sum, alen))
BUG();
if (unlikely(memcmp(esp->auth.work_icv, sum, alen))) {
x->stats.integrity_failed++;
goto out;
}
}
if ((nfrags = skb_cow_data(skb, 0, &trailer)) < 0)
goto out;
skb->ip_summed = CHECKSUM_NONE;
esph = (struct ip_esp_hdr*)skb->data;
/* Get ivec. This can be wrong, check against another impls. */
if (esp->conf.ivlen)
crypto_blkcipher_set_iv(tfm, esph->enc_data, esp->conf.ivlen);
sg = &esp->sgbuf[0];
if (unlikely(nfrags > ESP_NUM_FAST_SG)) {
sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC);
if (!sg)
goto out;
}
skb_to_sgvec(skb, sg, sizeof(struct ip_esp_hdr) + esp->conf.ivlen, elen);
err = crypto_blkcipher_decrypt(&desc, sg, sg, elen);
if (unlikely(sg != &esp->sgbuf[0]))
kfree(sg);
if (unlikely(err))
return err;
if (skb_copy_bits(skb, skb->len-alen-2, nexthdr, 2))
BUG();
padlen = nexthdr[0];
if (padlen+2 >= elen)
goto out;
/* ... check padding bits here. Silly. :-) */
iph = skb->nh.iph;
ihl = iph->ihl * 4;
if (x->encap) {
struct xfrm_encap_tmpl *encap = x->encap;
struct udphdr *uh = (void *)(skb->nh.raw + ihl);
/*
* 1) if the NAT-T peer's IP or port changed then
* advertize the change to the keying daemon.
* This is an inbound SA, so just compare
* SRC ports.
*/
if (iph->saddr != x->props.saddr.a4 ||
uh->source != encap->encap_sport) {
xfrm_address_t ipaddr;
ipaddr.a4 = iph->saddr;
km_new_mapping(x, &ipaddr, uh->source);
/* XXX: perhaps add an extra
* policy check here, to see
* if we should allow or
* reject a packet from a
* different source
* address/port.
*/
}
/*
* 2) ignore UDP/TCP checksums in case
* of NAT-T in Transport Mode, or
* perform other post-processing fixes
* as per draft-ietf-ipsec-udp-encaps-06,
* section 3.1.2
*/
if (x->props.mode == XFRM_MODE_TRANSPORT ||
x->props.mode == XFRM_MODE_BEET)
skb->ip_summed = CHECKSUM_UNNECESSARY;
}
iph->protocol = nexthdr[1];
pskb_trim(skb, skb->len - alen - padlen - 2);
skb->h.raw = __skb_pull(skb, sizeof(*esph) + esp->conf.ivlen) - ihl;
return 0;
out:
return -EINVAL;
}
/*
* Note: detecting truncated vs. non-truncated authentication data is very
* expensive, so we only support truncated data, which is the recommended
* and common case.
*/
static int esp_input(struct xfrm_state *x, struct xfrm_decap_state *decap, struct sk_buff *skb)
{
struct iphdr *iph;
struct ip_esp_hdr *esph;
struct esp_data *esp = x->data;
struct sk_buff *trailer;
int blksize = ALIGN(crypto_tfm_alg_blocksize(esp->conf.tfm), 4);
int alen = esp->auth.icv_trunc_len;
int elen = skb->len - sizeof(struct ip_esp_hdr) - esp->conf.ivlen - alen;
int nfrags;
int encap_len = 0;
u8 nexthdr[2];
struct scatterlist *sg;
u8 workbuf[60];
int padlen;
if (!pskb_may_pull(skb, sizeof(struct ip_esp_hdr)))
goto out;
esph = (struct ip_esp_hdr*)skb->data;
if (elen <= 0 || (elen & (blksize-1)))
goto out;
/* If integrity check is required, do this. */
if (esp->auth.icv_full_len) {
u8 sum[esp->auth.icv_full_len];
u8 sum1[alen];
if (x->props.replay_window && xfrm_replay_check(x, esph->seq_no))
goto out;
esp->auth.icv(esp, skb, 0, skb->len-alen, sum);
if (skb_copy_bits(skb, skb->len-alen, sum1, alen))
BUG();
if (unlikely(memcmp(sum, sum1, alen))) {
x->stats.integrity_failed++;
goto out;
}
if (x->props.replay_window)
xfrm_replay_advance(x, esph->seq_no);
}
if ((nfrags = skb_cow_data(skb, 0, &trailer)) < 0)
goto out;
skb->ip_summed = CHECKSUM_NONE;
iph = skb->nh.iph;
/* Get ivec. This can be wrong, check against another impls. */
if (esp->conf.ivlen)
crypto_cipher_set_iv(esp->conf.tfm, esph->enc_data, crypto_tfm_alg_ivsize(esp->conf.tfm));
sg = &esp->sgbuf[0];
if (unlikely(nfrags > ESP_NUM_FAST_SG)) {
sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC);
if (!sg)
goto out;
}
skb_to_sgvec(skb, sg, sizeof(struct ip_esp_hdr) + esp->conf.ivlen, elen);
crypto_cipher_decrypt(esp->conf.tfm, sg, sg, elen);
if (unlikely(sg != &esp->sgbuf[0]))
kfree(sg);
if (skb_copy_bits(skb, skb->len-alen-2, nexthdr, 2))
BUG();
padlen = nexthdr[0];
if (padlen+2 >= elen)
goto out;
/* ... check padding bits here. Silly. :-) */
if (x->encap) {
struct xfrm_encap_tmpl *encap = x->encap;
struct udphdr *uh;
if (encap->encap_type != decap->decap_type)
goto out;
uh = (struct udphdr *)(iph + 1);
encap_len = (void*)esph - (void*)uh;
/*
* 1) if the NAT-T peer's IP or port changed then
* advertize the change to the keying daemon.
* This is an inbound SA, so just compare
* SRC ports.
*/
if (iph->saddr != x->props.saddr.a4 ||
uh->source != encap->encap_sport) {
xfrm_address_t ipaddr;
ipaddr.a4 = iph->saddr;
km_new_mapping(x, &ipaddr, uh->source);
/* XXX: perhaps add an extra
* policy check here, to see
* if we should allow or
* reject a packet from a
* different source
* address/port.
*/
}
/*
* 2) ignore UDP/TCP checksums in case
* of NAT-T in Transport Mode, or
* perform other post-processing fixes
* as per draft-ietf-ipsec-udp-encaps-06,
* section 3.1.2
*/
if (!x->props.mode)
skb->ip_summed = CHECKSUM_UNNECESSARY;
}
iph->protocol = nexthdr[1];
pskb_trim(skb, skb->len - alen - padlen - 2);
memcpy(workbuf, skb->nh.raw, iph->ihl*4);
skb->h.raw = skb_pull(skb, sizeof(struct ip_esp_hdr) + esp->conf.ivlen);
skb->nh.raw += encap_len + sizeof(struct ip_esp_hdr) + esp->conf.ivlen;
memcpy(skb->nh.raw, workbuf, iph->ihl*4);
skb->nh.iph->tot_len = htons(skb->len);
return 0;
out:
return -EINVAL;
}
/*
* Note: detecting truncated vs. non-truncated authentication data is very
* expensive, so we only support truncated data, which is the recommended
* and common case.
*/
int esp_input(struct xfrm_state *x, struct xfrm_decap_state *decap, struct sk_buff *skb)
{
struct iphdr *iph;
struct ip_esp_hdr *esph;
struct esp_data *esp = x->data;
struct sk_buff *trailer;
int blksize = crypto_tfm_alg_blocksize(esp->conf.tfm);
int alen = esp->auth.icv_trunc_len;
int elen = skb->len - sizeof(struct ip_esp_hdr) - esp->conf.ivlen - alen;
int nfrags;
int encap_len = 0;
if (!pskb_may_pull(skb, sizeof(struct ip_esp_hdr)))
goto out;
if (elen <= 0 || (elen & (blksize-1)))
goto out;
/* If integrity check is required, do this. */
if (esp->auth.icv_full_len) {
u8 sum[esp->auth.icv_full_len];
u8 sum1[alen];
esp->auth.icv(esp, skb, 0, skb->len-alen, sum);
if (skb_copy_bits(skb, skb->len-alen, sum1, alen))
BUG();
if (unlikely(memcmp(sum, sum1, alen))) {
x->stats.integrity_failed++;
goto out;
}
}
if ((nfrags = skb_cow_data(skb, 0, &trailer)) < 0)
goto out;
skb->ip_summed = CHECKSUM_NONE;
esph = (struct ip_esp_hdr*)skb->data;
iph = skb->nh.iph;
/* Get ivec. This can be wrong, check against another impls. */
if (esp->conf.ivlen)
crypto_cipher_set_iv(esp->conf.tfm, esph->enc_data, crypto_tfm_alg_ivsize(esp->conf.tfm));
{
u8 nexthdr[2];
struct scatterlist sgbuf[nfrags>MAX_SG_ONSTACK ? 0 : nfrags];
struct scatterlist *sg = sgbuf;
u8 workbuf[60];
int padlen;
if (unlikely(nfrags > MAX_SG_ONSTACK)) {
sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC);
if (!sg)
goto out;
}
skb_to_sgvec(skb, sg, sizeof(struct ip_esp_hdr) + esp->conf.ivlen, elen);
crypto_cipher_decrypt(esp->conf.tfm, sg, sg, elen);
if (unlikely(sg != sgbuf))
kfree(sg);
if (skb_copy_bits(skb, skb->len-alen-2, nexthdr, 2))
BUG();
padlen = nexthdr[0];
if (padlen+2 >= elen)
goto out;
/* ... check padding bits here. Silly. :-) */
if (x->encap && decap && decap->decap_type) {
struct esp_decap_data *encap_data;
struct udphdr *uh = (struct udphdr *) (iph+1);
encap_data = (struct esp_decap_data *) (decap->decap_data);
encap_data->proto = 0;
switch (decap->decap_type) {
case UDP_ENCAP_ESPINUDP:
if ((void*)uh == (void*)esph) {
printk(KERN_DEBUG
"esp_input(): Got ESP; expecting ESPinUDP\n");
break;
}
encap_data->proto = AF_INET;
encap_data->saddr.a4 = iph->saddr;
encap_data->sport = uh->source;
encap_len = (void*)esph - (void*)uh;
if (encap_len != sizeof(*uh))
printk(KERN_DEBUG
"esp_input(): UDP -> ESP: too much room: %d\n",
encap_len);
break;
default:
printk(KERN_INFO
"esp_input(): processing unknown encap type: %u\n",
decap->decap_type);
break;
}
}
iph->protocol = nexthdr[1];
pskb_trim(skb, skb->len - alen - padlen - 2);
memcpy(workbuf, skb->nh.raw, iph->ihl*4);
skb->h.raw = skb_pull(skb, sizeof(struct ip_esp_hdr) + esp->conf.ivlen);
skb->nh.raw += encap_len + sizeof(struct ip_esp_hdr) + esp->conf.ivlen;
memcpy(skb->nh.raw, workbuf, iph->ihl*4);
skb->nh.iph->tot_len = htons(skb->len);
}
return 0;
out:
return -EINVAL;
}
static int nf_ct_frag6_queue(struct nf_ct_frag6_queue *fq, struct sk_buff *skb,
struct frag_hdr *fhdr, int nhoff)
{
struct sk_buff *prev, *next;
int offset, end;
if (fq->last_in & COMPLETE) {
DEBUGP("Allready completed\n");
goto err;
}
offset = ntohs(fhdr->frag_off) & ~0x7;
end = offset + (ntohs(skb->nh.ipv6h->payload_len) -
((u8 *) (fhdr + 1) - (u8 *) (skb->nh.ipv6h + 1)));
if ((unsigned int)end > IPV6_MAXPLEN) {
DEBUGP("offset is too large.\n");
return -1;
}
if (skb->ip_summed == CHECKSUM_HW)
skb->csum = csum_sub(skb->csum,
csum_partial(skb->nh.raw,
(u8*)(fhdr + 1) - skb->nh.raw,
0));
/* Is this the final fragment? */
if (!(fhdr->frag_off & htons(IP6_MF))) {
/* If we already have some bits beyond end
* or have different end, the segment is corrupted.
*/
if (end < fq->len ||
((fq->last_in & LAST_IN) && end != fq->len)) {
DEBUGP("already received last fragment\n");
goto err;
}
fq->last_in |= LAST_IN;
fq->len = end;
} else {
/* Check if the fragment is rounded to 8 bytes.
* Required by the RFC.
*/
if (end & 0x7) {
/* RFC2460 says always send parameter problem in
* this case. -DaveM
*/
DEBUGP("the end of this fragment is not rounded to 8 bytes.\n");
return -1;
}
if (end > fq->len) {
/* Some bits beyond end -> corruption. */
if (fq->last_in & LAST_IN) {
DEBUGP("last packet already reached.\n");
goto err;
}
fq->len = end;
}
}
if (end == offset)
goto err;
/* Point into the IP datagram 'data' part. */
if (!pskb_pull(skb, (u8 *) (fhdr + 1) - skb->data)) {
DEBUGP("queue: message is too short.\n");
goto err;
}
if (end-offset < skb->len) {
if (pskb_trim(skb, end - offset)) {
DEBUGP("Can't trim\n");
goto err;
}
if (skb->ip_summed != CHECKSUM_UNNECESSARY)
skb->ip_summed = CHECKSUM_NONE;
}
/* Find out which fragments are in front and at the back of us
* in the chain of fragments so far. We must know where to put
* this fragment, right?
*/
prev = NULL;
for (next = fq->fragments; next != NULL; next = next->next) {
if (NFCT_FRAG6_CB(next)->offset >= offset)
break; /* bingo! */
prev = next;
}
/* We found where to put this one. Check for overlap with
* preceding fragment, and, if needed, align things so that
* any overlaps are eliminated.
*/
if (prev) {
int i = (NFCT_FRAG6_CB(prev)->offset + prev->len) - offset;
if (i > 0) {
offset += i;
if (end <= offset) {
DEBUGP("overlap\n");
goto err;
}
if (!pskb_pull(skb, i)) {
DEBUGP("Can't pull\n");
goto err;
}
if (skb->ip_summed != CHECKSUM_UNNECESSARY)
skb->ip_summed = CHECKSUM_NONE;
}
}
/* Look for overlap with succeeding segments.
* If we can merge fragments, do it.
*/
while (next && NFCT_FRAG6_CB(next)->offset < end) {
/* overlap is 'i' bytes */
int i = end - NFCT_FRAG6_CB(next)->offset;
if (i < next->len) {
/* Eat head of the next overlapped fragment
* and leave the loop. The next ones cannot overlap.
*/
DEBUGP("Eat head of the overlapped parts.: %d", i);
if (!pskb_pull(next, i))
goto err;
/* next fragment */
NFCT_FRAG6_CB(next)->offset += i;
fq->meat -= i;
if (next->ip_summed != CHECKSUM_UNNECESSARY)
next->ip_summed = CHECKSUM_NONE;
break;
} else {
struct sk_buff *free_it = next;
/* Old fragmnet is completely overridden with
* new one drop it.
*/
next = next->next;
if (prev)
prev->next = next;
else
fq->fragments = next;
fq->meat -= free_it->len;
frag_kfree_skb(free_it, NULL);
}
}
NFCT_FRAG6_CB(skb)->offset = offset;
/* Insert this fragment in the chain of fragments. */
skb->next = next;
if (prev)
prev->next = skb;
else
fq->fragments = skb;
skb->dev = NULL;
skb_get_timestamp(skb, &fq->stamp);
fq->meat += skb->len;
atomic_add(skb->truesize, &nf_ct_frag6_mem);
/* The first fragment.
* nhoffset is obtained from the first fragment, of course.
*/
if (offset == 0) {
fq->nhoffset = nhoff;
fq->last_in |= FIRST_IN;
}
write_lock(&nf_ct_frag6_lock);
list_move_tail(&fq->lru_list, &nf_ct_frag6_lru_list);
write_unlock(&nf_ct_frag6_lock);
return 0;
err:
return -1;
}
static int esp6_input(struct xfrm_state *x, struct xfrm_decap_state *decap, struct sk_buff *skb)
{
struct ipv6hdr *iph;
struct ipv6_esp_hdr *esph;
struct esp_data *esp = x->data;
struct sk_buff *trailer;
int blksize = ALIGN(crypto_tfm_alg_blocksize(esp->conf.tfm), 4);
int alen = esp->auth.icv_trunc_len;
int elen = skb->len - sizeof(struct ipv6_esp_hdr) - esp->conf.ivlen - alen;
int hdr_len = skb->h.raw - skb->nh.raw;
int nfrags;
unsigned char *tmp_hdr = NULL;
int ret = 0;
if (!pskb_may_pull(skb, sizeof(struct ipv6_esp_hdr))) {
ret = -EINVAL;
goto out_nofree;
}
esph = (struct ipv6_esp_hdr*)skb->data;
if (elen <= 0 || (elen & (blksize-1))) {
ret = -EINVAL;
goto out_nofree;
}
tmp_hdr = kmalloc(hdr_len, GFP_ATOMIC);
if (!tmp_hdr) {
ret = -ENOMEM;
goto out_nofree;
}
memcpy(tmp_hdr, skb->nh.raw, hdr_len);
/* If integrity check is required, do this. */
if (esp->auth.icv_full_len) {
u8 sum[esp->auth.icv_full_len];
u8 sum1[alen];
if (x->props.replay_window && xfrm_replay_check(x, esph->seq_no)) {
ret = -EINVAL;
goto out;
}
esp->auth.icv(esp, skb, 0, skb->len-alen, sum);
if (skb_copy_bits(skb, skb->len-alen, sum1, alen))
BUG();
if (unlikely(memcmp(sum, sum1, alen))) {
x->stats.integrity_failed++;
ret = -EINVAL;
goto out;
}
if (x->props.replay_window)
xfrm_replay_advance(x, esph->seq_no);
}
if ((nfrags = skb_cow_data(skb, 0, &trailer)) < 0) {
ret = -EINVAL;
goto out;
}
skb->ip_summed = CHECKSUM_NONE;
iph = skb->nh.ipv6h;
/* Get ivec. This can be wrong, check against another impls. */
if (esp->conf.ivlen)
crypto_cipher_set_iv(esp->conf.tfm, esph->enc_data, crypto_tfm_alg_ivsize(esp->conf.tfm));
{
u8 nexthdr[2];
struct scatterlist *sg = &esp->sgbuf[0];
u8 padlen;
if (unlikely(nfrags > ESP_NUM_FAST_SG)) {
sg = kmalloc(sizeof(struct scatterlist)*nfrags, GFP_ATOMIC);
if (!sg) {
ret = -ENOMEM;
goto out;
}
}
skb_to_sgvec(skb, sg, sizeof(struct ipv6_esp_hdr) + esp->conf.ivlen, elen);
crypto_cipher_decrypt(esp->conf.tfm, sg, sg, elen);
if (unlikely(sg != &esp->sgbuf[0]))
kfree(sg);
if (skb_copy_bits(skb, skb->len-alen-2, nexthdr, 2))
BUG();
padlen = nexthdr[0];
if (padlen+2 >= elen) {
LIMIT_NETDEBUG(KERN_WARNING "ipsec esp packet is garbage padlen=%d, elen=%d\n", padlen+2, elen);
ret = -EINVAL;
goto out;
}
/* ... check padding bits here. Silly. :-) */
pskb_trim(skb, skb->len - alen - padlen - 2);
skb->h.raw = skb_pull(skb, sizeof(struct ipv6_esp_hdr) + esp->conf.ivlen);
skb->nh.raw += sizeof(struct ipv6_esp_hdr) + esp->conf.ivlen;
memcpy(skb->nh.raw, tmp_hdr, hdr_len);
skb->nh.ipv6h->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
ret = nexthdr[1];
}
out:
kfree(tmp_hdr);
out_nofree:
return ret;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
int i;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
PN_PRINTK("PN rcv: hdr rdev %x sdev %x res %x robj %x sobj %x dev=%s\n",
ph->pn_rdev, ph->pn_sdev, ph->pn_res,
ph->pn_robj, ph->pn_sobj, dev->name);
PN_DATA_PRINTK("PHONET : skb data = %d\nPHONET :", skb->len);
for (i = 1; i <= skb->len; i++) {
PN_DATA_PRINTK(" %02x", skb->data[i-1]);
if ((i%8) == 0)
PN_DATA_PRINTK("\n");
}
/* check if this is multicasted */
if (pn_sockaddr_get_object(&sa) == PNOBJECT_MULTICAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* check if this is broadcasted */
if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* resource routing */
if (pn_sockaddr_get_object(&sa) == 0) {
struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource);
if (sk) {
printk(KERN_DEBUG "phonet new resource routing!\n");
return sk_receive_skb(sk, skb, 0);
}
}
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
/*!*/ struct sock *sk = pn_find_sock_by_sa_and_skb(net, &sa, skb);
/*struct sock *sk = pn_find_sock_by_sa(net, &sa);*/
if (sk)
return sk_receive_skb(sk, skb, 0);
if (can_respond(skb)) {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
} else if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
goto out; /* Race between address deletion and loopback */
else {
/* Phonet packet routing */
struct net_device *out_dev;
out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa));
if (!out_dev) {
LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n",
pn_sockaddr_get_addr(&sa));
goto out;
}
__skb_push(skb, sizeof(struct phonethdr));
skb->dev = out_dev;
if (out_dev == dev) {
LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n",
pn_sockaddr_get_addr(&sa), dev->name);
goto out_dev;
}
/* Some drivers (e.g. TUN) do not allocate HW header space */
if (skb_cow_head(skb, out_dev->hard_header_len))
goto out_dev;
if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL,
skb->len) < 0)
goto out_dev;
dev_queue_xmit(skb);
dev_put(out_dev);
return NET_RX_SUCCESS;
out_dev:
dev_put(out_dev);
}
out:
kfree_skb(skb);
printk(KERN_DEBUG "phonet_rcv Drop message!\n");
return NET_RX_DROP;
}
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;
}
static int mptp_rcv(struct sk_buff *skb)
{
struct mptphdr *shdr;
struct mptp_sock *ssk;
__be16 len;
uint16_t src, dst;
struct sockaddr_mptp *mptp_addr;
int err;
int addr_size = sizeof(struct sockaddr_mptp) + sizeof(struct mptp_dest);
if (unlikely(!pskb_may_pull(skb, sizeof(struct mptphdr)))) {
log_error("Insufficient space for header\n");
goto drop;
}
shdr = (struct mptphdr *)skb->data;
len = ntohs(shdr->len);
if (unlikely(skb->len < len)) {
log_error("Malformed packet (packet_len=%u, skb_len=%u)\n", len,
skb->len);
goto drop;
}
if (unlikely(len < sizeof(struct mptphdr))) {
log_error
("Malformed packet (packet_len=%u sizeof(mptphdr)=%u\n",
len, sizeof(struct mptphdr));
goto drop;
}
src = ntohs(shdr->src);
dst = ntohs(shdr->dst);
if (unlikely
(src == 0 || dst == 0 || src >= MAX_MPTP_PORT
|| dst >= MAX_MPTP_PORT)) {
log_error("Malformed packet (src=%u, dst=%u)\n", shdr->src,
shdr->dst);
goto drop;
}
skb_pull(skb, sizeof(struct mptphdr));
len -= sizeof(struct mptphdr);
pskb_trim(skb, len);
log_debug("Received %u bytes from from port=%u to port=%u\n",
len - sizeof(struct mptphdr), src, dst);
ssk = mptp_lookup(dst);
if (ssk == NULL) {
log_error("MPTP lookup failed for port %u\n", dst);
goto drop;
}
BUG_ON(addr_size > sizeof(skb->cb));
mptp_addr = (struct sockaddr_mptp *)skb->cb;
mptp_addr->dests[0].port = shdr->src;
mptp_addr->dests[0].addr = ip_hdr(skb)->saddr;
log_debug("Setting sin_port=%u, sin_addr=%u\n", ntohs(shdr->src),
mptp_addr->dests[0].addr);
#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 36)
err = ip_queue_rcv_skb((struct sock *)&ssk->sock, skb);
#else
err = sock_queue_rcv_skb((struct sock *)&ssk->sock, skb);
#endif
if (unlikely(err)) {
log_error("ip_queue_rcv_skb failed with %d\n", err);
consume_skb(skb);
}
return NET_RX_SUCCESS;
drop:
kfree(skb);
return NET_RX_DROP;
}
