static unsigned int
fw_in(unsigned int hooknum,
struct sk_buff **pskb,
const struct net_device *in,
const struct net_device *out,
int (*okfn)(struct sk_buff *))
{
int ret = FW_BLOCK;
u_int16_t redirpt;
/* Assume worse case: any hook could change packet */
(*pskb)->nfcache |= NFC_UNKNOWN | NFC_ALTERED;
if ((*pskb)->ip_summed == CHECKSUM_HW)
(*pskb)->ip_summed = CHECKSUM_NONE;
/* Firewall rules can alter TOS: raw socket (tcpdump) may have
clone of incoming skb: don't disturb it --RR */
if (skb_cloned(*pskb) && !(*pskb)->sk) {
struct sk_buff *nskb = skb_copy(*pskb, GFP_ATOMIC);
if (!nskb)
return NF_DROP;
kfree_skb(*pskb);
*pskb = nskb;
}
switch (hooknum) {
case NF_IP_PRE_ROUTING:
if (fwops->fw_acct_in)
fwops->fw_acct_in(fwops, PF_INET,
(struct net_device *)in,
(*pskb)->nh.raw, &redirpt, pskb);
if ((*pskb)->nh.iph->frag_off & htons(IP_MF|IP_OFFSET)) {
*pskb = ip_ct_gather_frags(*pskb, IP_DEFRAG_CONNTRACK_IN);
if (!*pskb)
return NF_STOLEN;
}
ret = fwops->fw_input(fwops, PF_INET, (struct net_device *)in,
(*pskb)->nh.raw, &redirpt, pskb);
break;
case NF_IP_FORWARD:
/* Connection will only be set if it was
demasqueraded: if so, skip forward chain. */
if ((*pskb)->nfct)
ret = FW_ACCEPT;
else ret = fwops->fw_forward(fwops, PF_INET,
(struct net_device *)out,
(*pskb)->nh.raw, &redirpt, pskb);
break;
case NF_IP_POST_ROUTING:
ret = fwops->fw_output(fwops, PF_INET,
(struct net_device *)out,
(*pskb)->nh.raw, &redirpt, pskb);
if (ret == FW_ACCEPT || ret == FW_SKIP) {
if (fwops->fw_acct_out)
fwops->fw_acct_out(fwops, PF_INET,
(struct net_device *)out,
(*pskb)->nh.raw, &redirpt,
pskb);
/* ip_conntrack_confirm return NF_DROP or NF_ACCEPT */
if (ip_conntrack_confirm(hooknum, *pskb) == NF_DROP)
ret = FW_BLOCK;
}
break;
}
switch (ret) {
case FW_REJECT: {
/* Alexey says:
*
* Generally, routing is THE FIRST thing to make, when
* packet enters IP stack. Before packet is routed you
* cannot call any service routines from IP stack. */
struct iphdr *iph = (*pskb)->nh.iph;
if ((*pskb)->dst != NULL
|| ip_route_input(*pskb, iph->daddr, iph->saddr, iph->tos,
(struct net_device *)in) == 0)
icmp_send(*pskb, ICMP_DEST_UNREACH, ICMP_PORT_UNREACH,
0);
return NF_DROP;
}
case FW_ACCEPT:
case FW_SKIP:
if (hooknum == NF_IP_PRE_ROUTING) {
check_for_demasq(pskb);
check_for_redirect(*pskb);
} else if (hooknum == NF_IP_POST_ROUTING) {
check_for_unredirect(*pskb);
/* Handle ICMP errors from client here */
if ((*pskb)->nh.iph->protocol == IPPROTO_ICMP
&& (*pskb)->nfct)
check_for_masq_error(*pskb);
}
return NF_ACCEPT;
case FW_MASQUERADE:
if (hooknum == NF_IP_FORWARD) {
#ifdef CONFIG_IP_VS
/* check if it is for ip_vs */
if (check_for_ip_vs_out(pskb, okfn) == NF_STOLEN)
return NF_STOLEN;
#endif
return do_masquerade(pskb, out);
}
else return NF_ACCEPT;
case FW_REDIRECT:
if (hooknum == NF_IP_PRE_ROUTING)
return do_redirect(*pskb, in, redirpt);
else return NF_ACCEPT;
default:
/* FW_BLOCK */
return NF_DROP;
}
}
static struct sk_buff *vl600_tx_fixup(struct usbnet *dev,
struct sk_buff *skb, gfp_t flags)
{
struct sk_buff *ret;
struct vl600_frame_hdr *frame;
struct vl600_pkt_hdr *packet;
static uint32_t serial = 1;
int orig_len = skb->len - sizeof(struct ethhdr);
int full_len = (skb->len + sizeof(struct vl600_frame_hdr) + 3) & ~3;
frame = (struct vl600_frame_hdr *) skb->data;
if (skb->len > sizeof(*frame) && skb->len == le32_to_cpup(&frame->len))
return skb; /* Already encapsulated? */
if (skb->len < sizeof(struct ethhdr))
/* Drop, device can only deal with ethernet packets */
return NULL;
if (!skb_cloned(skb)) {
int headroom = skb_headroom(skb);
int tailroom = skb_tailroom(skb);
if (tailroom >= full_len - skb->len - sizeof(*frame) &&
headroom >= sizeof(*frame))
/* There's enough head and tail room */
goto encapsulate;
if (headroom + tailroom + skb->len >= full_len) {
/* There's enough total room, just readjust */
skb->data = memmove(skb->head + sizeof(*frame),
skb->data, skb->len);
skb_set_tail_pointer(skb, skb->len);
goto encapsulate;
}
}
/* Alloc a new skb with the required size */
ret = skb_copy_expand(skb, sizeof(struct vl600_frame_hdr), full_len -
skb->len - sizeof(struct vl600_frame_hdr), flags);
dev_kfree_skb_any(skb);
if (!ret)
return ret;
skb = ret;
encapsulate:
/* Packet header is same size as ethernet packet header
* (sizeof(*packet) == sizeof(struct ethhdr)), additionally the
* h_proto field is in the same place so we just leave it alone and
* overwrite the remaining fields.
*/
packet = (struct vl600_pkt_hdr *) skb->data;
/* The VL600 wants IPv6 packets to have an IPv4 ethertype
* Since this modem only supports IPv4 and IPv6, just set all
* frames to 0x0800 (ETH_P_IP)
*/
packet->h_proto = htons(ETH_P_IP);
memset(&packet->dummy, 0, sizeof(packet->dummy));
packet->len = cpu_to_le32(orig_len);
frame = (struct vl600_frame_hdr *) skb_push(skb, sizeof(*frame));
memset(frame, 0, sizeof(*frame));
frame->len = cpu_to_le32(full_len);
frame->serial = cpu_to_le32(serial++);
frame->pkt_cnt = cpu_to_le32(1);
if (skb->len < full_len) /* Pad */
skb_put(skb, full_len - skb->len);
return skb;
}
static int ip6_fragment(struct sk_buff *skb, int (*output)(struct sk_buff *))
{
struct net_device *dev;
struct sk_buff *frag;
struct rt6_info *rt = (struct rt6_info*)skb->dst;
struct ipv6_pinfo *np = skb->sk ? inet6_sk(skb->sk) : NULL;
struct ipv6hdr *tmp_hdr;
struct frag_hdr *fh;
unsigned int mtu, hlen, left, len;
__be32 frag_id = 0;
int ptr, offset = 0, err=0;
u8 *prevhdr, nexthdr = 0;
dev = rt->u.dst.dev;
hlen = ip6_find_1stfragopt(skb, &prevhdr);
nexthdr = *prevhdr;
mtu = ip6_skb_dst_mtu(skb);
/* We must not fragment if the socket is set to force MTU discovery
* or if the skb it not generated by a local socket. (This last
* check should be redundant, but it's free.)
*/
if (!np || np->pmtudisc >= IPV6_PMTUDISC_DO) {
skb->dev = skb->dst->dev;
icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu, skb->dev);
IP6_INC_STATS(ip6_dst_idev(skb->dst), IPSTATS_MIB_FRAGFAILS);
kfree_skb(skb);
return -EMSGSIZE;
}
if (np && np->frag_size < mtu) {
if (np->frag_size)
mtu = np->frag_size;
}
mtu -= hlen + sizeof(struct frag_hdr);
if (skb_shinfo(skb)->frag_list) {
int first_len = skb_pagelen(skb);
int truesizes = 0;
if (first_len - hlen > mtu ||
((first_len - hlen) & 7) ||
skb_cloned(skb))
goto slow_path;
for (frag = skb_shinfo(skb)->frag_list; frag; frag = frag->next) {
/* Correct geometry. */
if (frag->len > mtu ||
((frag->len & 7) && frag->next) ||
skb_headroom(frag) < hlen)
goto slow_path;
/* Partially cloned skb? */
if (skb_shared(frag))
goto slow_path;
BUG_ON(frag->sk);
if (skb->sk) {
sock_hold(skb->sk);
frag->sk = skb->sk;
frag->destructor = sock_wfree;
truesizes += frag->truesize;
}
}
err = 0;
offset = 0;
frag = skb_shinfo(skb)->frag_list;
skb_shinfo(skb)->frag_list = NULL;
/* BUILD HEADER */
*prevhdr = NEXTHDR_FRAGMENT;
tmp_hdr = kmemdup(skb_network_header(skb), hlen, GFP_ATOMIC);
if (!tmp_hdr) {
IP6_INC_STATS(ip6_dst_idev(skb->dst), IPSTATS_MIB_FRAGFAILS);
return -ENOMEM;
}
__skb_pull(skb, hlen);
fh = (struct frag_hdr*)__skb_push(skb, sizeof(struct frag_hdr));
__skb_push(skb, hlen);
skb_reset_network_header(skb);
memcpy(skb_network_header(skb), tmp_hdr, hlen);
ipv6_select_ident(skb, fh);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(IP6_MF);
frag_id = fh->identification;
first_len = skb_pagelen(skb);
skb->data_len = first_len - skb_headlen(skb);
skb->truesize -= truesizes;
skb->len = first_len;
ipv6_hdr(skb)->payload_len = htons(first_len -
sizeof(struct ipv6hdr));
dst_hold(&rt->u.dst);
for (;;) {
/* Prepare header of the next frame,
* before previous one went down. */
if (frag) {
frag->ip_summed = CHECKSUM_NONE;
skb_reset_transport_header(frag);
fh = (struct frag_hdr*)__skb_push(frag, sizeof(struct frag_hdr));
__skb_push(frag, hlen);
skb_reset_network_header(frag);
memcpy(skb_network_header(frag), tmp_hdr,
hlen);
offset += skb->len - hlen - sizeof(struct frag_hdr);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(offset);
if (frag->next != NULL)
fh->frag_off |= htons(IP6_MF);
fh->identification = frag_id;
ipv6_hdr(frag)->payload_len =
htons(frag->len -
sizeof(struct ipv6hdr));
ip6_copy_metadata(frag, skb);
}
err = output(skb);
if(!err)
IP6_INC_STATS(ip6_dst_idev(&rt->u.dst), IPSTATS_MIB_FRAGCREATES);
if (err || !frag)
break;
skb = frag;
frag = skb->next;
skb->next = NULL;
}
kfree(tmp_hdr);
if (err == 0) {
IP6_INC_STATS(ip6_dst_idev(&rt->u.dst), IPSTATS_MIB_FRAGOKS);
dst_release(&rt->u.dst);
return 0;
}
while (frag) {
skb = frag->next;
kfree_skb(frag);
frag = skb;
}
IP6_INC_STATS(ip6_dst_idev(&rt->u.dst), IPSTATS_MIB_FRAGFAILS);
dst_release(&rt->u.dst);
return err;
}
slow_path:
left = skb->len - hlen; /* Space per frame */
ptr = hlen; /* Where to start from */
/*
* Fragment the datagram.
*/
*prevhdr = NEXTHDR_FRAGMENT;
/*
* Keep copying data until we run out.
*/
while(left > 0) {
len = left;
/* IF: it doesn't fit, use 'mtu' - the data space left */
if (len > mtu)
len = mtu;
/* IF: we are not sending upto and including the packet end
then align the next start on an eight byte boundary */
if (len < left) {
len &= ~7;
}
/*
* Allocate buffer.
*/
if ((frag = alloc_skb(len+hlen+sizeof(struct frag_hdr)+LL_RESERVED_SPACE(rt->u.dst.dev), GFP_ATOMIC)) == NULL) {
NETDEBUG(KERN_INFO "IPv6: frag: no memory for new fragment!\n");
IP6_INC_STATS(ip6_dst_idev(skb->dst),
IPSTATS_MIB_FRAGFAILS);
err = -ENOMEM;
goto fail;
}
/*
* Set up data on packet
*/
ip6_copy_metadata(frag, skb);
skb_reserve(frag, LL_RESERVED_SPACE(rt->u.dst.dev));
skb_put(frag, len + hlen + sizeof(struct frag_hdr));
skb_reset_network_header(frag);
fh = (struct frag_hdr *)(skb_network_header(frag) + hlen);
frag->transport_header = (frag->network_header + hlen +
sizeof(struct frag_hdr));
/*
* Charge the memory for the fragment to any owner
* it might possess
*/
if (skb->sk)
skb_set_owner_w(frag, skb->sk);
/*
* Copy the packet header into the new buffer.
*/
skb_copy_from_linear_data(skb, skb_network_header(frag), hlen);
/*
* Build fragment header.
*/
fh->nexthdr = nexthdr;
fh->reserved = 0;
if (!frag_id) {
ipv6_select_ident(skb, fh);
frag_id = fh->identification;
} else
fh->identification = frag_id;
/*
* Copy a block of the IP datagram.
*/
if (skb_copy_bits(skb, ptr, skb_transport_header(frag), len))
BUG();
left -= len;
fh->frag_off = htons(offset);
if (left > 0)
fh->frag_off |= htons(IP6_MF);
ipv6_hdr(frag)->payload_len = htons(frag->len -
sizeof(struct ipv6hdr));
ptr += len;
offset += len;
/*
* Put this fragment into the sending queue.
*/
err = output(frag);
if (err)
goto fail;
IP6_INC_STATS(ip6_dst_idev(skb->dst), IPSTATS_MIB_FRAGCREATES);
}
IP6_INC_STATS(ip6_dst_idev(skb->dst),
IPSTATS_MIB_FRAGOKS);
kfree_skb(skb);
return err;
fail:
IP6_INC_STATS(ip6_dst_idev(skb->dst),
IPSTATS_MIB_FRAGFAILS);
kfree_skb(skb);
return err;
}
static int ip_frag_reasm(struct ipq *qp, struct sk_buff *prev,
struct net_device *dev)
{
struct net *net = container_of(qp->q.net, struct net, ipv4.frags);
struct iphdr *iph;
struct sk_buff *fp, *head = qp->q.fragments;
int len;
int ihlen;
int err;
u8 ecn;
ipq_kill(qp);
ecn = ip4_frag_ecn_table[qp->ecn];
if (unlikely(ecn == 0xff)) {
err = -EINVAL;
goto out_fail;
}
/* Make the one we just received the head. */
if (prev) {
head = prev->next;
fp = skb_clone(head, GFP_ATOMIC);
if (!fp)
goto out_nomem;
fp->next = head->next;
if (!fp->next)
qp->q.fragments_tail = fp;
prev->next = fp;
skb_morph(head, qp->q.fragments);
head->next = qp->q.fragments->next;
kfree_skb(qp->q.fragments);
qp->q.fragments = head;
}
WARN_ON(head == NULL);
WARN_ON(FRAG_CB(head)->offset != 0);
/* Allocate a new buffer for the datagram. */
ihlen = ip_hdrlen(head);
len = ihlen + qp->q.len;
err = -E2BIG;
if (len > 65535)
goto out_oversize;
/* Head of list must not be cloned. */
if (skb_cloned(head) && pskb_expand_head(head, 0, 0, GFP_ATOMIC))
goto out_nomem;
/* If the first fragment is fragmented itself, we split
* it to two chunks: the first with data and paged part
* and the second, holding only fragments. */
if (skb_has_frag_list(head)) {
struct sk_buff *clone;
int i, plen = 0;
if ((clone = alloc_skb(0, GFP_ATOMIC)) == NULL)
goto out_nomem;
clone->next = head->next;
head->next = clone;
skb_shinfo(clone)->frag_list = skb_shinfo(head)->frag_list;
skb_frag_list_init(head);
for (i=0; i<skb_shinfo(head)->nr_frags; i++)
plen += skb_shinfo(head)->frags[i].size;
clone->len = clone->data_len = head->data_len - plen;
head->data_len -= clone->len;
head->len -= clone->len;
clone->csum = 0;
clone->ip_summed = head->ip_summed;
atomic_add(clone->truesize, &qp->q.net->mem);
}
skb_shinfo(head)->frag_list = head->next;
skb_push(head, head->data - skb_network_header(head));
for (fp=head->next; fp; fp = fp->next) {
head->data_len += fp->len;
head->len += fp->len;
if (head->ip_summed != fp->ip_summed)
head->ip_summed = CHECKSUM_NONE;
else if (head->ip_summed == CHECKSUM_COMPLETE)
head->csum = csum_add(head->csum, fp->csum);
head->truesize += fp->truesize;
}
atomic_sub(head->truesize, &qp->q.net->mem);
head->next = NULL;
head->dev = dev;
head->tstamp = qp->q.stamp;
iph = ip_hdr(head);
iph->frag_off = 0;
iph->tot_len = htons(len);
iph->tos |= ecn;
IP_INC_STATS_BH(net, IPSTATS_MIB_REASMOKS);
qp->q.fragments = NULL;
qp->q.fragments_tail = NULL;
return 0;
out_nomem:
LIMIT_NETDEBUG(KERN_ERR "IP: queue_glue: no memory for gluing "
"queue %p\n", qp);
err = -ENOMEM;
goto out_fail;
out_oversize:
if (net_ratelimit())
printk(KERN_INFO "Oversized IP packet from %pI4.\n",
&qp->saddr);
out_fail:
IP_INC_STATS_BH(net, IPSTATS_MIB_REASMFAILS);
return err;
}
int xfrm4_rcv_encap(struct sk_buff *skb, __u16 encap_type)
{
int err;
u32 spi, seq;
struct sec_decap_state xfrm_vec[XFRM_MAX_DEPTH];
struct xfrm_state *x;
int xfrm_nr = 0;
int decaps = 0;
int offset = 0;
if ((err = xfrm4_parse_spi(skb, skb->nh.iph->protocol, &spi, &seq)) != 0)
goto drop;
do {
struct iphdr *iph = skb->nh.iph;
if (xfrm_nr == XFRM_MAX_DEPTH)
goto drop;
x = xfrm_state_lookup((xfrm_address_t *)&iph->daddr, spi, iph->protocol, AF_INET);
if (x == NULL)
goto drop;
spin_lock(&x->lock);
if (unlikely(x->km.state != XFRM_STATE_VALID))
goto drop_unlock;
#if defined(CONFIG_CAVIUM_OCTEON_IPSEC) && defined(CONFIG_NET_KEY)
/*
* If Octeon IPSEC Acceleration module has been loaded
* call it, otherwise, follow the software path
*/
if(cavium_ipsec_process)
{
if (xfrm_state_check_expire(x))
goto drop_unlock;
xfrm_vec[xfrm_nr].decap.decap_type = encap_type;
switch (skb->nh.iph->protocol) {
case IPPROTO_AH:
offset = offsetof(struct ip_auth_hdr, nexthdr);
break;
case IPPROTO_ESP:
offset = offsetof(struct ip_esp_hdr, spi);
break;
default:
return -skb->nh.iph->protocol;
}
offset += (uint64_t)skb->data - (uint64_t)skb->nh.iph;
/*
* skb->data points to the start of the esp/ah header
* but we require skb->data to point to the start of ip header.
*/
skb_push(skb, (unsigned int)((uint64_t)skb->data - (uint64_t)skb->nh.iph));
if (skb_is_nonlinear(skb) &&
skb_linearize(skb, GFP_ATOMIC) != 0) {
err = -ENOMEM;
goto drop_unlock;
}
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
{
err = -ENOMEM;
goto drop_unlock;
}
err = cavium_ipsec_process(x->sa_handle, skb, offset, 0 /*DECRYPT*/);
if(err)
goto drop_unlock;
/* only the first xfrm gets the encap type */
encap_type = 0;
x->curlft.bytes += skb->len;
x->curlft.packets++;
spin_unlock(&x->lock);
xfrm_vec[xfrm_nr++].xvec = x;
iph = skb->nh.iph;
if (x->props.mode) {
#if 0
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
/*
* TODO, support ECN and dscp decapsulation
*/
if (x->props.flags & XFRM_STATE_DECAP_DSCP)
ipv4_copy_dscp(iph, skb->h.ipiph);
if (!(x->props.flags & XFRM_STATE_NOECN))
ipip_ecn_decapsulate(skb);
#endif
skb->nh.raw = skb->data;
memset(&(IPCB(skb)->opt), 0, sizeof(struct ip_options));
decaps = 1;
break;
}
} else { /* if (cavium_ipsec_process == NULL) */
if (x->props.replay_window && xfrm_replay_check(x, seq))
goto drop_unlock;
if (xfrm_state_check_expire(x))
goto drop_unlock;
xfrm_vec[xfrm_nr].decap.decap_type = encap_type;
if (x->type->input(x, &(xfrm_vec[xfrm_nr].decap), skb))
goto drop_unlock;
/* only the first xfrm gets the encap type */
encap_type = 0;
if (x->props.replay_window)
xfrm_replay_advance(x, seq);
x->curlft.bytes += skb->len;
x->curlft.packets++;
spin_unlock(&x->lock);
xfrm_vec[xfrm_nr++].xvec = x;
iph = skb->nh.iph;
if (x->props.mode) {
if (iph->protocol != IPPROTO_IPIP)
goto drop;
if (!pskb_may_pull(skb, sizeof(struct iphdr)))
goto drop;
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
if (x->props.flags & XFRM_STATE_DECAP_DSCP)
ipv4_copy_dscp(iph, skb->h.ipiph);
if (!(x->props.flags & XFRM_STATE_NOECN))
ipip_ecn_decapsulate(skb);
skb->mac.raw = memmove(skb->data - skb->mac_len,
skb->mac.raw, skb->mac_len);
skb->nh.raw = skb->data;
memset(&(IPCB(skb)->opt), 0, sizeof(struct ip_options));
decaps = 1;
break;
}
} /* if (cavium_ipsec_process) */
#else
if (x->props.replay_window && xfrm_replay_check(x, seq))
goto drop_unlock;
if (xfrm_state_check_expire(x))
goto drop_unlock;
xfrm_vec[xfrm_nr].decap.decap_type = encap_type;
if (x->type->input(x, &(xfrm_vec[xfrm_nr].decap), skb))
goto drop_unlock;
/* only the first xfrm gets the encap type */
encap_type = 0;
if (x->props.replay_window)
xfrm_replay_advance(x, seq);
x->curlft.bytes += skb->len;
x->curlft.packets++;
spin_unlock(&x->lock);
xfrm_vec[xfrm_nr++].xvec = x;
iph = skb->nh.iph;
if (x->props.mode) {
if (iph->protocol != IPPROTO_IPIP)
goto drop;
if (!pskb_may_pull(skb, sizeof(struct iphdr)))
goto drop;
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
if (x->props.flags & XFRM_STATE_DECAP_DSCP)
ipv4_copy_dscp(iph, skb->h.ipiph);
if (!(x->props.flags & XFRM_STATE_NOECN))
ipip_ecn_decapsulate(skb);
skb->mac.raw = memmove(skb->data - skb->mac_len,
skb->mac.raw, skb->mac_len);
skb->nh.raw = skb->data;
memset(&(IPCB(skb)->opt), 0, sizeof(struct ip_options));
decaps = 1;
break;
}
#endif
if ((err = xfrm_parse_spi(skb, skb->nh.iph->protocol, &spi, &seq)) < 0)
goto drop;
} while (!err);
void sendPSDData(int cid, struct sk_buff *skb)
{
struct pduhdr *hdr;
struct sk_buff *skb2;
unsigned len;
unsigned tailpad;
unsigned long flags;
len = skb->len;
tailpad = padding_size(sizeof *hdr + len);
if (likely(!skb_cloned(skb))) {
int headroom = skb_headroom(skb);
int tailroom = skb_tailroom(skb);
/* enough room as-is? */
if (likely(sizeof *hdr + tailpad <= headroom + tailroom)) {
/* do not need to be readjusted */
if(sizeof *hdr <= headroom && tailpad <= tailroom)
goto fill;
skb->data = memmove(skb->head + sizeof *hdr,
skb->data, len);
skb_set_tail_pointer(skb, len);
goto fill;
}
}
/* create a new skb, with the correct size (and tailpad) */
skb2 = skb_copy_expand(skb, sizeof *hdr, tailpad + 1, GFP_ATOMIC);
dev_kfree_skb_any(skb);
if (unlikely(!skb2))
return;
skb = skb2;
/* fill out the pdu header */
fill:
hdr = (void *) __skb_push(skb, sizeof *hdr);
memset(hdr, 0, sizeof *hdr);
hdr->length = cpu_to_be16(len);
hdr->cid = cid;
memset(skb_put(skb, tailpad), 0, tailpad);
/* link is down */
if(!data_path_is_link_up()) {
dev_kfree_skb_any(skb);
return;
}
spin_lock_irqsave(&tx_q_lock, flags);
/* drop the packet if queue is full */
if (skb_queue_len(&tx_q) >= PSD_TX_QUEUE_MAX_LEN) {
struct sk_buff *first = skb_dequeue(&tx_q);
dev_kfree_skb_any(first);
}
skb_queue_tail(&tx_q, skb);
spin_unlock_irqrestore(&tx_q_lock, flags);
data_path_schedule_tx(psd_dp);
}
static int ah6_input(struct xfrm_state *x, struct xfrm_decap_state *decap, struct sk_buff *skb)
{
/*
* Before process AH
* [IPv6][Ext1][Ext2][AH][Dest][Payload]
* |<-------------->| hdr_len
*
* To erase AH:
* Keeping copy of cleared headers. After AH processing,
* Moving the pointer of skb->nh.raw by using skb_pull as long as AH
* header length. Then copy back the copy as long as hdr_len
* If destination header following AH exists, copy it into after [Ext2].
*
* |<>|[IPv6][Ext1][Ext2][Dest][Payload]
* There is offset of AH before IPv6 header after the process.
*/
struct ipv6_auth_hdr *ah;
struct ah_data *ahp;
unsigned char *tmp_hdr = NULL;
u16 hdr_len;
u16 ah_hlen;
int nexthdr;
if (!pskb_may_pull(skb, sizeof(struct ip_auth_hdr)))
goto out;
/* We are going to _remove_ AH header to keep sockets happy,
* so... Later this can change. */
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto out;
hdr_len = skb->data - skb->nh.raw;
ah = (struct ipv6_auth_hdr*)skb->data;
ahp = x->data;
nexthdr = ah->nexthdr;
ah_hlen = (ah->hdrlen + 2) << 2;
if (ah_hlen != XFRM_ALIGN8(sizeof(struct ipv6_auth_hdr) + ahp->icv_full_len) &&
ah_hlen != XFRM_ALIGN8(sizeof(struct ipv6_auth_hdr) + ahp->icv_trunc_len))
goto out;
if (!pskb_may_pull(skb, ah_hlen))
goto out;
tmp_hdr = kmalloc(hdr_len, GFP_ATOMIC);
if (!tmp_hdr)
goto out;
memcpy(tmp_hdr, skb->nh.raw, hdr_len);
if (ipv6_clear_mutable_options(skb->nh.ipv6h, hdr_len))
goto out;
skb->nh.ipv6h->priority = 0;
skb->nh.ipv6h->flow_lbl[0] = 0;
skb->nh.ipv6h->flow_lbl[1] = 0;
skb->nh.ipv6h->flow_lbl[2] = 0;
skb->nh.ipv6h->hop_limit = 0;
{
u8 auth_data[MAX_AH_AUTH_LEN];
memcpy(auth_data, ah->auth_data, ahp->icv_trunc_len);
memset(ah->auth_data, 0, ahp->icv_trunc_len);
skb_push(skb, skb->data - skb->nh.raw);
ahp->icv(ahp, skb, ah->auth_data);
if (memcmp(ah->auth_data, auth_data, ahp->icv_trunc_len)) {
LIMIT_NETDEBUG(
printk(KERN_WARNING "ipsec ah authentication error\n"));
x->stats.integrity_failed++;
goto free_out;
}
}
skb->nh.raw = skb_pull(skb, ah_hlen);
memcpy(skb->nh.raw, tmp_hdr, hdr_len);
skb->nh.ipv6h->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
skb_pull(skb, hdr_len);
skb->h.raw = skb->data;
kfree(tmp_hdr);
return nexthdr;
free_out:
kfree(tmp_hdr);
out:
return -EINVAL;
}
static struct sk_buff *
kalmia_tx_fixup(struct usbnet *dev, struct sk_buff *skb, gfp_t flags)
{
struct sk_buff *skb2 = NULL;
u16 content_len;
unsigned char *header_start;
unsigned char ether_type_1, ether_type_2;
u8 remainder, padlen = 0;
if (!skb_cloned(skb)) {
int headroom = skb_headroom(skb);
int tailroom = skb_tailroom(skb);
if ((tailroom >= KALMIA_ALIGN_SIZE) && (headroom
>= KALMIA_HEADER_LENGTH))
goto done;
if ((headroom + tailroom) > (KALMIA_HEADER_LENGTH
+ KALMIA_ALIGN_SIZE)) {
skb->data = memmove(skb->head + KALMIA_HEADER_LENGTH,
skb->data, skb->len);
skb_set_tail_pointer(skb, skb->len);
goto done;
}
}
skb2 = skb_copy_expand(skb, KALMIA_HEADER_LENGTH,
KALMIA_ALIGN_SIZE, flags);
if (!skb2)
return NULL;
dev_kfree_skb_any(skb);
skb = skb2;
done:
header_start = skb_push(skb, KALMIA_HEADER_LENGTH);
ether_type_1 = header_start[KALMIA_HEADER_LENGTH + 12];
ether_type_2 = header_start[KALMIA_HEADER_LENGTH + 13];
netdev_dbg(dev->net, "Sending etherType: %02x%02x", ether_type_1,
ether_type_2);
/* According to empiric data for data packages */
header_start[0] = 0x57;
header_start[1] = 0x44;
content_len = skb->len - KALMIA_HEADER_LENGTH;
put_unaligned_le16(content_len, &header_start[2]);
header_start[4] = ether_type_1;
header_start[5] = ether_type_2;
/* Align to 4 bytes by padding with zeros */
remainder = skb->len % KALMIA_ALIGN_SIZE;
if (remainder > 0) {
padlen = KALMIA_ALIGN_SIZE - remainder;
memset(skb_put(skb, padlen), 0, padlen);
}
netdev_dbg(
dev->net,
"Sending package with length %i and padding %i. Header: %02x:%02x:%02x:%02x:%02x:%02x.",
content_len, padlen, header_start[0], header_start[1],
header_start[2], header_start[3], header_start[4],
header_start[5]);
return skb;
}
static struct sk_buff *
mvswitch_mangle_tx(struct net_device *dev, struct sk_buff *skb)
{
struct mvswitch_priv *priv;
char *buf = NULL;
u16 vid;
priv = dev->phy_ptr;
if (unlikely(!priv))
goto error;
if (unlikely(skb->len < 16))
goto error;
#ifdef HEADER_MODE
if (__vlan_hwaccel_get_tag(skb, &vid))
goto error;
if (skb_cloned(skb) || (skb->len <= 62) || (skb_headroom(skb) < MV_HEADER_SIZE)) {
if (pskb_expand_head(skb, MV_HEADER_SIZE, (skb->len < 62 ? 62 - skb->len : 0), GFP_ATOMIC))
goto error_expand;
if (skb->len < 62)
skb->len = 62;
}
buf = skb_push(skb, MV_HEADER_SIZE);
#else
if (__vlan_get_tag(skb, &vid))
goto error;
if (unlikely((vid > 15 || !priv->vlans[vid])))
goto error;
if (skb->len <= 64) {
if (pskb_expand_head(skb, 0, 64 + MV_TRAILER_SIZE - skb->len, GFP_ATOMIC))
goto error_expand;
buf = skb->data + 64;
skb->len = 64 + MV_TRAILER_SIZE;
} else {
if (skb_cloned(skb) || unlikely(skb_tailroom(skb) < 4)) {
if (pskb_expand_head(skb, 0, 4, GFP_ATOMIC))
goto error_expand;
}
buf = skb_put(skb, 4);
}
/* move the ethernet header 4 bytes forward, overwriting the vlan tag */
memmove(skb->data + 4, skb->data, 12);
skb->data += 4;
skb->len -= 4;
skb->mac_header += 4;
#endif
if (!buf)
goto error;
#ifdef HEADER_MODE
/* prepend the tag */
*((__be16 *) buf) = cpu_to_be16(
((vid << MV_HEADER_VLAN_S) & MV_HEADER_VLAN_M) |
((priv->vlans[vid] << MV_HEADER_PORTS_S) & MV_HEADER_PORTS_M)
);
#else
/* append the tag */
*((__be32 *) buf) = cpu_to_be32((
(MV_TRAILER_OVERRIDE << MV_TRAILER_FLAGS_S) |
((priv->vlans[vid] & MV_TRAILER_PORTS_M) << MV_TRAILER_PORTS_S)
));
#endif
return skb;
error_expand:
if (net_ratelimit())
printk("%s: failed to expand/update skb for the switch\n", dev->name);
error:
/* any errors? drop the packet! */
dev_kfree_skb_any(skb);
return NULL;
}
static int ipip_tunnel_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct ip_tunnel *tunnel = (struct ip_tunnel*)dev->priv;
struct net_device_stats *stats = &tunnel->stat;
struct iphdr *tiph = &tunnel->parms.iph;
u8 tos = tunnel->parms.iph.tos;
u16 df = tiph->frag_off;
struct rtable *rt; /* Route to the other host */
struct net_device *tdev; /* Device to other host */
struct iphdr *old_iph = skb->nh.iph;
#ifdef CONFIG_NET_IPIP_IPV6
struct ipv6hdr *iph6 = skb->nh.ipv6h;
#endif
struct iphdr *iph; /* Our new IP header */
int max_headroom; /* The extra header space needed */
u32 dst = tiph->daddr;
int mtu;
u8 protocol = 0;
switch (skb->protocol) {
case __constant_htons(ETH_P_IP):
protocol = IPPROTO_IPIP;
break;
#ifdef CONFIG_NET_IPIP_IPV6
case __constant_htons(ETH_P_IPV6):
protocol = IPPROTO_IPV6;
break;
#endif
}
if (tunnel->recursion++) {
tunnel->stat.collisions++;
goto tx_error;
}
switch(skb->protocol) {
case __constant_htons(ETH_P_IP):
if (tunnel->parms.iph.protocol &&
tunnel->parms.iph.protocol != IPPROTO_IPIP)
goto tx_error;
if (tos&1)
tos = old_iph->tos;
break;
#ifdef CONFIG_NET_IPIP_IPV6
case __constant_htons(ETH_P_IPV6):
if (tunnel->parms.iph.protocol &&
tunnel->parms.iph.protocol != IPPROTO_IPV6)
goto tx_error;
break;
#endif
default:
goto tx_error;
}
if (!dst) {
switch(skb->protocol){
case __constant_htons(ETH_P_IP):
/* NBMA tunnel */
if ((rt = (struct rtable*)skb->dst) == NULL) {
tunnel->stat.tx_fifo_errors++;
goto tx_error;
}
dst = rt->rt_gateway;
break;
#ifdef CONFIG_NET_IPIP_IPV6
case __constant_htons(ETH_P_IPV6):
{
struct in6_addr *addr6 = &iph6->daddr;
if (addr6->s6_addr16[0] == htons(0x2002)) {
memcpy(&dst, &addr6->s6_addr16[1], 4);
} else {
/* dst is zero */
struct neighbour *neigh = NULL;
if (skb->dst)
neigh = skb->dst->neighbour;
if (neigh == NULL) {
printk(KERN_DEBUG "tunl: nexthop == NULL\n");
goto tx_error;
}
addr6 = (struct in6_addr*)&neigh->primary_key;
if (IN6_IS_ADDR_UNSPECIFIED(addr6))
addr6 = &skb->nh.ipv6h->daddr;
if (IN6_IS_ADDR_V4COMPAT(addr6))
dst = addr6->s6_addr32[3];
#ifdef CONFIG_IPV6_6TO4_NEXTHOP
else if (addr6->s6_addr16[0] == htons(0x2002))
memcpy(&dst, &addr6->s6_addr16[1], 4);
#endif
else
goto tx_error_icmp;
}
break;
}
#endif
}
if (!dst)
goto tx_error_icmp;
}
if (ip_route_output(&rt, dst, tiph->saddr, RT_TOS(tos), tunnel->parms.link)) {
tunnel->stat.tx_carrier_errors++;
goto tx_error_icmp;
}
tdev = rt->u.dst.dev;
if (tdev == dev) {
ip_rt_put(rt);
tunnel->stat.collisions++;
goto tx_error;
}
if (tiph->frag_off)
mtu = rt->u.dst.pmtu - sizeof(struct iphdr);
else
mtu = skb->dst ? skb->dst->pmtu : dev->mtu;
if (mtu < 68) {
tunnel->stat.collisions++;
ip_rt_put(rt);
goto tx_error;
}
switch(skb->protocol){
case __constant_htons(ETH_P_IP):
if (skb->dst && mtu < skb->dst->pmtu)
skb->dst->pmtu = mtu;
df |= (old_iph->frag_off&htons(IP_DF));
if ((old_iph->frag_off&htons(IP_DF)) && mtu < ntohs(old_iph->tot_len)) {
icmp_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED, htonl(mtu));
ip_rt_put(rt);
goto tx_error;
}
break;
#ifdef CONFIG_NET_IPIP_IPV6
case __constant_htons(ETH_P_IPV6):
#if 0
if (mtu < IPV6_MIN_MTU) {
/* XXX: too small; we should fragment this packet? */
tunnel->stat.tx_carrier_errors++;
goto tx_error_icmp;
}
#endif
if (skb->len > mtu && mtu > IPV6_MIN_MTU) {
icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu, dev);
ip_rt_put(rt);
goto tx_error;
}
df = mtu > IPV6_MIN_MTU ? htons(IP_DF) : 0;
break;
#endif
}
if (tunnel->err_count > 0) {
if (jiffies - tunnel->err_time < IPTUNNEL_ERR_TIMEO) {
tunnel->err_count--;
dst_link_failure(skb);
} else
tunnel->err_count = 0;
}
/*
* Okay, now see if we can stuff it in the buffer as-is.
*/
max_headroom = (((tdev->hard_header_len+15)&~15)+sizeof(struct iphdr));
if (skb_headroom(skb) < max_headroom || skb_cloned(skb) || skb_shared(skb)) {
struct sk_buff *new_skb = skb_realloc_headroom(skb, max_headroom);
if (!new_skb) {
ip_rt_put(rt);
stats->tx_dropped++;
dev_kfree_skb(skb);
tunnel->recursion--;
return 0;
}
if (skb->sk)
skb_set_owner_w(new_skb, skb->sk);
dev_kfree_skb(skb);
skb = new_skb;
old_iph = skb->nh.iph;
}
skb->h.raw = skb->nh.raw;
skb->nh.raw = skb_push(skb, sizeof(struct iphdr));
memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
dst_release(skb->dst);
skb->dst = &rt->u.dst;
/*
* Push down and install the IPIP header.
*/
iph = skb->nh.iph;
iph->version = 4;
iph->ihl = sizeof(struct iphdr)>>2;
iph->daddr = rt->rt_dst;
iph->saddr = rt->rt_src;
iph->ttl = tiph->ttl;
iph->frag_off = df;
switch(skb->protocol){
case __constant_htons(ETH_P_IP):
iph->protocol = protocol;
iph->tos = INET_ECN_encapsulate(tos, old_iph->tos);
if (iph->ttl == 0)
iph->ttl = old_iph->ttl;
break;
#ifdef CONFIG_NET_IPIP_IPV6
case __constant_htons(ETH_P_IPV6):
iph->protocol = protocol;
iph->tos = INET_ECN_encapsulate(tos, ip6_get_dsfield(iph6));
if (iph->ttl == 0)
iph->ttl = iph6->hop_limit;
break;
#endif
}
nf_reset(skb);
IPTUNNEL_XMIT();
tunnel->recursion--;
return 0;
tx_error_icmp:
dst_link_failure(skb);
tx_error:
stats->tx_errors++;
dev_kfree_skb(skb);
tunnel->recursion--;
return 0;
}
int xfrm4_rcv_encap(struct sk_buff *skb, __u16 encap_type)
{
int err;
u32 spi, seq;
struct sec_decap_state xfrm_vec[XFRM_MAX_DEPTH];
struct xfrm_state *x;
int xfrm_nr = 0;
int decaps = 0;
if ((err = xfrm4_parse_spi(skb, skb->nh.iph->protocol, &spi, &seq)) != 0)
goto drop;
do {
struct iphdr *iph = skb->nh.iph;
if (xfrm_nr == XFRM_MAX_DEPTH)
goto drop;
x = xfrm_state_lookup((xfrm_address_t *)&iph->daddr, spi, iph->protocol, AF_INET);
if (x == NULL)
goto drop;
spin_lock(&x->lock);
if (unlikely(x->km.state != XFRM_STATE_VALID))
goto drop_unlock;
if (x->props.replay_window && xfrm_replay_check(x, seq))
goto drop_unlock;
if (xfrm_state_check_expire(x))
goto drop_unlock;
xfrm_vec[xfrm_nr].decap.decap_type = encap_type;
if (x->type->input(x, &(xfrm_vec[xfrm_nr].decap), skb))
goto drop_unlock;
/* only the first xfrm gets the encap type */
encap_type = 0;
if (x->props.replay_window)
xfrm_replay_advance(x, seq);
x->curlft.bytes += skb->len;
x->curlft.packets++;
spin_unlock(&x->lock);
xfrm_vec[xfrm_nr++].xvec = x;
iph = skb->nh.iph;
if (x->props.mode) {
if (iph->protocol != IPPROTO_IPIP)
goto drop;
if (!pskb_may_pull(skb, sizeof(struct iphdr)))
goto drop;
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
if (x->props.flags & XFRM_STATE_DECAP_DSCP)
ipv4_copy_dscp(iph, skb->h.ipiph);
if (!(x->props.flags & XFRM_STATE_NOECN))
ipip_ecn_decapsulate(skb);
skb->mac.raw = memmove(skb->data - skb->mac_len,
skb->mac.raw, skb->mac_len);
skb->nh.raw = skb->data;
memset(&(IPCB(skb)->opt), 0, sizeof(struct ip_options));
decaps = 1;
break;
}
if ((err = xfrm_parse_spi(skb, skb->nh.iph->protocol, &spi, &seq)) < 0)
goto drop;
} while (!err);
/* Allocate new secpath or COW existing one. */
if (!skb->sp || atomic_read(&skb->sp->refcnt) != 1) {
struct sec_path *sp;
sp = secpath_dup(skb->sp);
if (!sp)
goto drop;
if (skb->sp)
secpath_put(skb->sp);
skb->sp = sp;
}
if (xfrm_nr + skb->sp->len > XFRM_MAX_DEPTH)
goto drop;
memcpy(skb->sp->x+skb->sp->len, xfrm_vec, xfrm_nr*sizeof(struct sec_decap_state));
skb->sp->len += xfrm_nr;
nf_reset(skb);
if (decaps) {
if (!(skb->dev->flags&IFF_LOOPBACK)) {
dst_release(skb->dst);
skb->dst = NULL;
}
netif_rx(skb);
return 0;
} else {
#ifdef CONFIG_NETFILTER
__skb_push(skb, skb->data - skb->nh.raw);
skb->nh.iph->tot_len = htons(skb->len);
ip_send_check(skb->nh.iph);
NF_HOOK(PF_INET, NF_IP_PRE_ROUTING, skb, skb->dev, NULL,
xfrm4_rcv_encap_finish);
return 0;
#else
return -skb->nh.iph->protocol;
#endif
}
drop_unlock:
spin_unlock(&x->lock);
xfrm_state_put(x);
drop:
while (--xfrm_nr >= 0)
xfrm_state_put(xfrm_vec[xfrm_nr].xvec);
kfree_skb(skb);
return 0;
}
static int ip6_frag_reasm(struct frag_queue *fq, struct sk_buff *prev,
struct net_device *dev)
{
struct net *net = container_of(fq->q.net, struct net, ipv6.frags);
struct sk_buff *fp, *head = fq->q.fragments;
int payload_len;
unsigned int nhoff;
fq_kill(fq);
if (prev) {
head = prev->next;
fp = skb_clone(head, GFP_ATOMIC);
if (!fp)
goto out_oom;
fp->next = head->next;
if (!fp->next)
fq->q.fragments_tail = fp;
prev->next = fp;
skb_morph(head, fq->q.fragments);
head->next = fq->q.fragments->next;
kfree_skb(fq->q.fragments);
fq->q.fragments = head;
}
WARN_ON(head == NULL);
WARN_ON(FRAG6_CB(head)->offset != 0);
payload_len = ((head->data - skb_network_header(head)) -
sizeof(struct ipv6hdr) + fq->q.len -
sizeof(struct frag_hdr));
if (payload_len > IPV6_MAXPLEN)
goto out_oversize;
if (skb_cloned(head) && pskb_expand_head(head, 0, 0, GFP_ATOMIC))
goto out_oom;
if (skb_has_frag_list(head)) {
struct sk_buff *clone;
int i, plen = 0;
if ((clone = alloc_skb(0, GFP_ATOMIC)) == NULL)
goto out_oom;
clone->next = head->next;
head->next = clone;
skb_shinfo(clone)->frag_list = skb_shinfo(head)->frag_list;
skb_frag_list_init(head);
for (i = 0; i < skb_shinfo(head)->nr_frags; i++)
plen += skb_frag_size(&skb_shinfo(head)->frags[i]);
clone->len = clone->data_len = head->data_len - plen;
head->data_len -= clone->len;
head->len -= clone->len;
clone->csum = 0;
clone->ip_summed = head->ip_summed;
atomic_add(clone->truesize, &fq->q.net->mem);
}
nhoff = fq->nhoffset;
skb_network_header(head)[nhoff] = skb_transport_header(head)[0];
memmove(head->head + sizeof(struct frag_hdr), head->head,
(head->data - head->head) - sizeof(struct frag_hdr));
head->mac_header += sizeof(struct frag_hdr);
head->network_header += sizeof(struct frag_hdr);
skb_shinfo(head)->frag_list = head->next;
skb_reset_transport_header(head);
skb_push(head, head->data - skb_network_header(head));
for (fp=head->next; fp; fp = fp->next) {
head->data_len += fp->len;
head->len += fp->len;
if (head->ip_summed != fp->ip_summed)
head->ip_summed = CHECKSUM_NONE;
else if (head->ip_summed == CHECKSUM_COMPLETE)
head->csum = csum_add(head->csum, fp->csum);
head->truesize += fp->truesize;
}
atomic_sub(head->truesize, &fq->q.net->mem);
head->next = NULL;
head->dev = dev;
head->tstamp = fq->q.stamp;
ipv6_hdr(head)->payload_len = htons(payload_len);
IP6CB(head)->nhoff = nhoff;
if (head->ip_summed == CHECKSUM_COMPLETE)
head->csum = csum_partial(skb_network_header(head),
skb_network_header_len(head),
head->csum);
rcu_read_lock();
IP6_INC_STATS_BH(net, __in6_dev_get(dev), IPSTATS_MIB_REASMOKS);
rcu_read_unlock();
fq->q.fragments = NULL;
fq->q.fragments_tail = NULL;
return 1;
out_oversize:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: payload len = %d\n", payload_len);
goto out_fail;
out_oom:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: no memory for reassembly\n");
out_fail:
rcu_read_lock();
IP6_INC_STATS_BH(net, __in6_dev_get(dev), IPSTATS_MIB_REASMFAILS);
rcu_read_unlock();
return -1;
}
static int ah_input(struct xfrm_state *x, struct xfrm_decap_state *decap, struct sk_buff *skb)
{
int ah_hlen;
struct iphdr *iph;
struct ip_auth_hdr *ah;
struct ah_data *ahp;
char work_buf[60];
if (!pskb_may_pull(skb, sizeof(struct ip_auth_hdr)))
goto out;
ah = (struct ip_auth_hdr*)skb->data;
if (x->props.replay_window && xfrm_replay_check(x, ah->seq_no))
goto out;
ahp = x->data;
ah_hlen = (ah->hdrlen + 2) << 2;
if (ah_hlen != XFRM_ALIGN8(sizeof(struct ip_auth_hdr) + ahp->icv_full_len) &&
ah_hlen != XFRM_ALIGN8(sizeof(struct ip_auth_hdr) + ahp->icv_trunc_len))
goto out;
if (!pskb_may_pull(skb, ah_hlen))
goto out;
/* We are going to _remove_ AH header to keep sockets happy,
* so... Later this can change. */
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto out;
skb->ip_summed = CHECKSUM_NONE;
ah = (struct ip_auth_hdr*)skb->data;
iph = skb->nh.iph;
memcpy(work_buf, iph, iph->ihl*4);
iph->ttl = 0;
iph->tos = 0;
iph->frag_off = 0;
iph->check = 0;
if (iph->ihl != 5) {
u32 dummy;
if (ip_clear_mutable_options(iph, &dummy))
goto out;
}
{
u8 auth_data[MAX_AH_AUTH_LEN];
memcpy(auth_data, ah->auth_data, ahp->icv_trunc_len);
skb_push(skb, skb->data - skb->nh.raw);
ahp->icv(ahp, skb, ah->auth_data);
if (memcmp(ah->auth_data, auth_data, ahp->icv_trunc_len)) {
x->stats.integrity_failed++;
goto out;
}
}
if (x->props.replay_window)
xfrm_replay_advance(x, ah->seq_no);
((struct iphdr*)work_buf)->protocol = ah->nexthdr;
skb->nh.raw = skb_pull(skb, ah_hlen);
memcpy(skb->nh.raw, work_buf, iph->ihl*4);
skb->nh.iph->tot_len = htons(skb->len);
skb_pull(skb, skb->nh.iph->ihl*4);
skb->h.raw = skb->data;
return 0;
out:
return -EINVAL;
}
int skb_cow_data(struct sk_buff *skb, int tailbits, struct sk_buff **trailer)
{
int copyflag;
int elt;
struct sk_buff *skb1, **skb_p;
/* If skb is cloned or its head is paged, reallocate
* head pulling out all the pages (pages are considered not writable
* at the moment even if they are anonymous).
*/
if ((skb_cloned(skb) || skb_shinfo(skb)->nr_frags) &&
__pskb_pull_tail(skb, skb_pagelen(skb)-skb_headlen(skb)) == NULL)
return -ENOMEM;
/* Easy case. Most of packets will go this way. */
if (!skb_shinfo(skb)->frag_list) {
/* A little of trouble, not enough of space for trailer.
* This should not happen, when stack is tuned to generate
* good frames. OK, on miss we reallocate and reserve even more
* space, 128 bytes is fair. */
if (skb_tailroom(skb) < tailbits &&
pskb_expand_head(skb, 0, tailbits-skb_tailroom(skb)+128, GFP_ATOMIC))
return -ENOMEM;
/* Voila! */
*trailer = skb;
return 1;
}
/* Misery. We are in troubles, going to mincer fragments... */
elt = 1;
skb_p = &skb_shinfo(skb)->frag_list;
copyflag = 0;
while ((skb1 = *skb_p) != NULL) {
int ntail = 0;
/* The fragment is partially pulled by someone,
* this can happen on input. Copy it and everything
* after it. */
if (skb_shared(skb1))
copyflag = 1;
/* If the skb is the last, worry about trailer. */
if (skb1->next == NULL && tailbits) {
if (skb_shinfo(skb1)->nr_frags ||
skb_shinfo(skb1)->frag_list ||
skb_tailroom(skb1) < tailbits)
ntail = tailbits + 128;
}
if (copyflag ||
skb_cloned(skb1) ||
ntail ||
skb_shinfo(skb1)->nr_frags ||
skb_shinfo(skb1)->frag_list) {
struct sk_buff *skb2;
/* F**k, we are miserable poor guys... */
if (ntail == 0)
skb2 = skb_copy(skb1, GFP_ATOMIC);
else
skb2 = skb_copy_expand(skb1,
skb_headroom(skb1),
ntail,
GFP_ATOMIC);
if (unlikely(skb2 == NULL))
return -ENOMEM;
if (skb1->sk)
skb_set_owner_w(skb, skb1->sk);
/* Looking around. Are we still alive?
* OK, link new skb, drop old one */
skb2->next = skb1->next;
*skb_p = skb2;
kfree_skb(skb1);
skb1 = skb2;
}
elt++;
*trailer = skb1;
skb_p = &skb1->next;
}
return elt;
}
/*
* Check if this packet is complete.
* Returns NULL on failure by any reason, and pointer
* to current nexthdr field in reassembled frame.
*
* It is called with locked fq, and caller must check that
* queue is eligible for reassembly i.e. it is not COMPLETE,
* the last and the first frames arrived and all the bits are here.
*/
static int ip6_frag_reasm(struct frag_queue *fq, struct sk_buff *prev,
struct net_device *dev)
{
struct net *net = container_of(fq->q.net, struct net, ipv6.frags);
struct sk_buff *fp, *head = fq->q.fragments;
int payload_len;
unsigned int nhoff;
fq_kill(fq);
/* Make the one we just received the head. */
if (prev) {
head = prev->next;
fp = skb_clone(head, GFP_ATOMIC);
if (!fp)
goto out_oom;
fp->next = head->next;
prev->next = fp;
skb_morph(head, fq->q.fragments);
head->next = fq->q.fragments->next;
kfree_skb(fq->q.fragments);
fq->q.fragments = head;
}
WARN_ON(head == NULL);
WARN_ON(FRAG6_CB(head)->offset != 0);
/* Unfragmented part is taken from the first segment. */
payload_len = ((head->data - skb_network_header(head)) -
sizeof(struct ipv6hdr) + fq->q.len -
sizeof(struct frag_hdr));
if (payload_len > IPV6_MAXPLEN)
goto out_oversize;
/* Head of list must not be cloned. */
if (skb_cloned(head) && pskb_expand_head(head, 0, 0, GFP_ATOMIC))
goto out_oom;
/* If the first fragment is fragmented itself, we split
* it to two chunks: the first with data and paged part
* and the second, holding only fragments. */
if (skb_shinfo(head)->frag_list) {
struct sk_buff *clone;
int i, plen = 0;
if ((clone = alloc_skb(0, GFP_ATOMIC)) == NULL)
goto out_oom;
clone->next = head->next;
head->next = clone;
skb_shinfo(clone)->frag_list = skb_shinfo(head)->frag_list;
skb_shinfo(head)->frag_list = NULL;
for (i=0; i<skb_shinfo(head)->nr_frags; i++)
plen += skb_shinfo(head)->frags[i].size;
clone->len = clone->data_len = head->data_len - plen;
head->data_len -= clone->len;
head->len -= clone->len;
clone->csum = 0;
clone->ip_summed = head->ip_summed;
atomic_add(clone->truesize, &fq->q.net->mem);
}
/* We have to remove fragment header from datagram and to relocate
* header in order to calculate ICV correctly. */
nhoff = fq->nhoffset;
skb_network_header(head)[nhoff] = skb_transport_header(head)[0];
memmove(head->head + sizeof(struct frag_hdr), head->head,
(head->data - head->head) - sizeof(struct frag_hdr));
head->mac_header += sizeof(struct frag_hdr);
head->network_header += sizeof(struct frag_hdr);
skb_shinfo(head)->frag_list = head->next;
skb_reset_transport_header(head);
skb_push(head, head->data - skb_network_header(head));
atomic_sub(head->truesize, &fq->q.net->mem);
for (fp=head->next; fp; fp = fp->next) {
head->data_len += fp->len;
head->len += fp->len;
if (head->ip_summed != fp->ip_summed)
head->ip_summed = CHECKSUM_NONE;
else if (head->ip_summed == CHECKSUM_COMPLETE)
head->csum = csum_add(head->csum, fp->csum);
head->truesize += fp->truesize;
atomic_sub(fp->truesize, &fq->q.net->mem);
}
head->next = NULL;
head->dev = dev;
head->tstamp = fq->q.stamp;
ipv6_hdr(head)->payload_len = htons(payload_len);
IP6CB(head)->nhoff = nhoff;
/* Yes, and fold redundant checksum back. 8) */
if (head->ip_summed == CHECKSUM_COMPLETE)
head->csum = csum_partial(skb_network_header(head),
skb_network_header_len(head),
head->csum);
rcu_read_lock();
IP6_INC_STATS_BH(net, __in6_dev_get(dev), IPSTATS_MIB_REASMOKS);
rcu_read_unlock();
fq->q.fragments = NULL;
return 1;
out_oversize:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: payload len = %d\n", payload_len);
goto out_fail;
out_oom:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: no memory for reassembly\n");
out_fail:
rcu_read_lock();
IP6_INC_STATS_BH(net, __in6_dev_get(dev), IPSTATS_MIB_REASMFAILS);
rcu_read_unlock();
return -1;
}
static int
ip6ip6_tnl_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct ip6_tnl *t = (struct ip6_tnl *) dev->priv;
struct net_device_stats *stats = &t->stat;
struct ipv6hdr *ipv6h = skb->nh.ipv6h;
int encap_limit = -1;
struct ipv6_tel_txoption opt;
__u16 offset;
struct flowi fl;
struct dst_entry *dst;
struct net_device *tdev;
int mtu;
int max_headroom = sizeof(struct ipv6hdr);
u8 proto;
int err;
int pkt_len;
int dsfield;
if (t->recursion++) {
stats->collisions++;
goto tx_err;
}
if (skb->protocol != htons(ETH_P_IPV6) ||
!ip6_tnl_xmit_ctl(t) || ip6ip6_tnl_addr_conflict(t, ipv6h)) {
goto tx_err;
}
if ((offset = parse_tlv_tnl_enc_lim(skb, skb->nh.raw)) > 0) {
struct ipv6_tlv_tnl_enc_lim *tel;
tel = (struct ipv6_tlv_tnl_enc_lim *) &skb->nh.raw[offset];
if (tel->encap_limit == 0) {
icmpv6_send(skb, ICMPV6_PARAMPROB,
ICMPV6_HDR_FIELD, offset + 2, skb->dev);
goto tx_err;
}
encap_limit = tel->encap_limit - 1;
} else if (!(t->parms.flags & IP6_TNL_F_IGN_ENCAP_LIMIT)) {
encap_limit = t->parms.encap_limit;
}
memcpy(&fl, &t->fl, sizeof (fl));
fl.iif = ((struct inet6_skb_parm *)skb->cb)->iif;
proto = fl.proto;
dsfield = ipv6_get_dsfield(ipv6h);
if ((t->parms.flags & IP6_TNL_F_USE_ORIG_TCLASS))
fl.fl6_flowlabel |= (*(__u32 *) ipv6h & IPV6_TCLASS_MASK);
if ((t->parms.flags & IP6_TNL_F_USE_ORIG_FLOWLABEL))
fl.fl6_flowlabel |= (*(__u32 *) ipv6h & IPV6_FLOWLABEL_MASK);
if ((dst = ip6_tnl_dst_check(t)) != NULL)
dst_hold(dst);
else {
dst = ip6_route_output(NULL, &fl);
if (dst->error || xfrm_lookup(&dst, &fl, NULL, 0) < 0)
goto tx_err_link_failure;
}
tdev = dst->dev;
if (tdev == dev) {
stats->collisions++;
if (net_ratelimit())
printk(KERN_WARNING
"%s: Local routing loop detected!\n",
t->parms.name);
goto tx_err_dst_release;
}
mtu = dst_mtu(dst) - sizeof (*ipv6h);
if (encap_limit >= 0) {
max_headroom += 8;
mtu -= 8;
}
if (mtu < IPV6_MIN_MTU)
mtu = IPV6_MIN_MTU;
if (skb->dst && mtu < dst_mtu(skb->dst)) {
struct rt6_info *rt = (struct rt6_info *) skb->dst;
rt->rt6i_flags |= RTF_MODIFIED;
rt->u.dst.metrics[RTAX_MTU-1] = mtu;
}
if (skb->len > mtu) {
icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu, dev);
goto tx_err_dst_release;
}
/*
* Okay, now see if we can stuff it in the buffer as-is.
*/
max_headroom += LL_RESERVED_SPACE(tdev);
if (skb_headroom(skb) < max_headroom ||
skb_cloned(skb) || skb_shared(skb)) {
struct sk_buff *new_skb;
if (!(new_skb = skb_realloc_headroom(skb, max_headroom)))
goto tx_err_dst_release;
if (skb->sk)
skb_set_owner_w(new_skb, skb->sk);
kfree_skb(skb);
skb = new_skb;
}
dst_release(skb->dst);
skb->dst = dst_clone(dst);
skb->h.raw = skb->nh.raw;
if (encap_limit >= 0) {
init_tel_txopt(&opt, encap_limit);
ipv6_push_nfrag_opts(skb, &opt.ops, &proto, NULL);
}
skb->nh.raw = skb_push(skb, sizeof(struct ipv6hdr));
ipv6h = skb->nh.ipv6h;
*(u32*)ipv6h = fl.fl6_flowlabel | htonl(0x60000000);
dsfield = INET_ECN_encapsulate(0, dsfield);
ipv6_change_dsfield(ipv6h, ~INET_ECN_MASK, dsfield);
ipv6h->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
ipv6h->hop_limit = t->parms.hop_limit; /*XXX use physical link's mtu */
ipv6h->nexthdr = proto;
ipv6_addr_copy(&ipv6h->saddr, &fl.fl6_src);
ipv6_addr_copy(&ipv6h->daddr, &fl.fl6_dst);
nf_reset(skb);
pkt_len = skb->len;
err = NF_HOOK(PF_INET6, NF_IP6_LOCAL_OUT, skb, NULL,
skb->dst->dev, dst_output);
if (err == NET_XMIT_SUCCESS || err == NET_XMIT_CN) {
stats->tx_bytes += pkt_len;
stats->tx_packets++;
} else {
stats->tx_errors++;
stats->tx_aborted_errors++;
}
ip6_tnl_dst_store(t, dst);
t->recursion--;
return 0;
tx_err_link_failure:
stats->tx_carrier_errors++;
dst_link_failure(skb);
tx_err_dst_release:
dst_release(dst);
tx_err:
stats->tx_errors++;
stats->tx_dropped++;
kfree_skb(skb);
t->recursion--;
return 0;
}
int islpci_eth_transmit(struct sk_buff *skb, struct net_device *nwdev)
{
islpci_private *private_config = nwdev->priv;
isl38xx_control_block *control_block = private_config->control_block;
isl38xx_fragment *fragment;
u32 index, counter, pci_map_address;
int fragments;
int frame_size;
int offset;
struct sk_buff *newskb;
int newskb_offset;
unsigned long flags;
unsigned char wds_mac[6];
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_FUNCTION_CALLS, "islpci_eth_transmit \n");
#endif
// lock the driver code
driver_lock( &private_config->slock, &flags );
// check whether the deviec is running
// if( !netif_running( nwdev ) )
// {
// DEBUG( SHOW_ERROR_MESSAGES, "%s: Tx on stopped device!\n",
// nwdev->name);
// return 1;
// }
// tx_timeout_check( nwdev, islpci_eth_tx_timeout );
// skb_tx_check( nwdev, skb );
// determine the amount of fragments needed to store the frame
memset (wds_mac, 0, 6);
#ifdef WDS_LINKS
check_skb_for_wds(skb, wds_mac);
#ifdef ISLPCI_ETH_DEBUG
if ( wds_mac[0] || wds_mac[1] || wds_mac[2] || wds_mac[3] || wds_mac[4] || wds_mac[5] ) {
printk("islpci_eth_transmit:check_skb_for_wds ! %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
wds_mac[0], wds_mac[1], wds_mac[2], wds_mac[3], wds_mac[4], wds_mac[5] );
} else {
printk("islpci_eth_transmit:check_skb_for_wds %2.2X %2.2X %2.2X %2.2X %2.2X %2.2X\n",
wds_mac[0], wds_mac[1], wds_mac[2], wds_mac[3], wds_mac[4], wds_mac[5] );
}
#endif
#endif
frame_size = skb->len < ETH_ZLEN ? ETH_ZLEN : skb->len;
if( init_wds ) frame_size += 6;
fragments = frame_size / MAX_FRAGMENT_SIZE;
fragments += frame_size % MAX_FRAGMENT_SIZE == 0 ? 0 : 1;
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "Fragments needed for frame %i\n", fragments);
#endif
// check whether the destination queue has enough fragments for the frame
if (ISL38XX_CB_TX_QSIZE - isl38xx_in_queue( control_block,
ISL38XX_CB_TX_DATA_LQ ) < fragments )
{
// Error, cannot add the frame because the queue is full
DEBUG(SHOW_ERROR_MESSAGES, "Error: Queue [%i] not enough room\n",
ISL38XX_CB_TX_DATA_LQ);
// trigger the device by setting the Update bit in the Device Int reg
writel(ISL38XX_DEV_INT_UPDATE, private_config->remapped_device_base +
ISL38XX_DEV_INT_REG);
udelay(ISL38XX_WRITEIO_DELAY);
// unlock the driver code
driver_unlock( &private_config->slock, &flags );
return -EBUSY;
}
// Check alignment and WDS frame formatting. The start of the packet should
// be aligned on a 4-byte boundary. If WDS is enabled add another 6 bytes
// and add WDS address information
if( ((int) skb->data & 0x03) | init_wds )
{
// get the number of bytes to add and re-allign
offset = (int) skb->data & 0x03;
offset += init_wds ? 6 : 0;
// check whether the current skb can be used
if (!skb_cloned(skb) && (skb_tailroom(skb) >= offset))
{
unsigned char *src = skb->data;
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "skb offset %i wds %i\n", offset, init_wds );
#endif
// allign the buffer on 4-byte boundary
skb_reserve(skb, (int) skb->data & 0x03 );
if( init_wds )
{
// wds requires an additional address field of 6 bytes
skb_put( skb, 6 );
#ifdef ISLPCI_ETH_DEBUG
printk("islpci_eth_transmit:wds_mac\n" );
#endif
memmove( skb->data+6, src, skb->len );
memcpy( skb->data, wds_mac, 6 );
}
else
{
memmove( skb->data, src, skb->len );
}
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "memmove %p %p %i \n", skb->data, src,
skb->len );
#endif
}
else
{
newskb = dev_alloc_skb( init_wds ? skb->len+6 : skb->len );
newskb_offset = (int) newskb->data & 0x03;
// Check if newskb->data is aligned
if( newskb_offset )
skb_reserve(newskb, newskb_offset);
skb_put(newskb, init_wds ? skb->len+6 : skb->len );
if( init_wds )
{
memcpy( newskb->data+6, skb->data, skb->len );
memcpy( newskb->data, wds_mac, 6 );
#ifdef ISLPCI_ETH_DEBUG
printk("islpci_eth_transmit:wds_mac\n" );
#endif
}
else
memcpy( newskb->data, skb->data, skb->len );
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "memcpy %p %p %i wds %i\n", newskb->data,
skb->data, skb->len, init_wds );
#endif
newskb->dev = skb->dev;
dev_kfree_skb(skb);
skb = newskb;
}
}
// display the buffer contents for debugging
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG( SHOW_BUFFER_CONTENTS, "\ntx %p ", skb->data );
display_buffer((char *) skb->data, skb->len );
#endif
// map the skb buffer to pci memory for DMA operation
pci_map_address = pci_map_single( private_config->pci_device,
(void *) skb->data, skb->len, PCI_DMA_TODEVICE );
if( pci_map_address == (dma_addr_t) NULL )
{
// error mapping the buffer to device accessable memory address
DEBUG(SHOW_ERROR_MESSAGES, "Error map DMA addr, data lost\n" );
// free the skbuf structure before aborting
dev_kfree_skb(skb);
// unlock the driver code
driver_unlock( &private_config->slock, &flags );
return -EIO;
}
// place each fragment in the control block structure and store in the last
// needed fragment entry of the pci_map_tx_address and data_low_tx arrays
// the skb frame information
for (counter = 0; counter < fragments; counter++ )
{
// get a pointer to the target control block fragment
index = (counter + le32_to_cpu(
control_block->driver_curr_frag[ISL38XX_CB_TX_DATA_LQ])) %
ISL38XX_CB_TX_QSIZE;
fragment = &control_block->tx_data_low[index];
// check whether this frame fragment is the last one
if( counter == fragments-1 )
{
// the fragment is the last one, add the streaming DMA mapping for
// proper PCI bus operation
private_config->pci_map_tx_address[index] =
(dma_addr_t) pci_map_address;
// store the skb address for future freeing
private_config->data_low_tx[index] = skb;
}
else
{
// the fragment will be followed by more fragments
// clear the pci_map_tx_address and data_low_tx entries
private_config->pci_map_tx_address[index] = (dma_addr_t) NULL;
private_config->data_low_tx[index] = NULL;
}
// set the proper fragment start address and size information
fragment->address = cpu_to_le32( pci_map_address +
counter*MAX_FRAGMENT_SIZE );
fragment->size = cpu_to_le16( frame_size > MAX_FRAGMENT_SIZE ?
MAX_FRAGMENT_SIZE : frame_size );
fragment->flags = cpu_to_le16( frame_size > MAX_FRAGMENT_SIZE ? 1 : 0 );
frame_size -= MAX_FRAGMENT_SIZE;
}
// ready loading all fragements to the control block and setup pci mapping
// update the control block interface information
add_le32p(&control_block->driver_curr_frag[ISL38XX_CB_TX_DATA_LQ],
fragments);
// check whether the data tx queue is full now
if( ISL38XX_CB_TX_QSIZE - isl38xx_in_queue( control_block,
ISL38XX_CB_TX_DATA_LQ ) < ISL38XX_MIN_QTHRESHOLD )
{
// stop the transmission of network frames to the driver
netif_stop_queue(nwdev);
// set the full flag for the data low transmission queue
private_config->data_low_tx_full = 1;
}
// trigger the device
isl38xx_trigger_device( &private_config->powerstate,
private_config->remapped_device_base );
// set the transmission time
nwdev->trans_start = jiffies;
private_config->statistics.tx_packets++;
private_config->statistics.tx_bytes += skb->len;
// cleanup the data low transmit queue
islpci_eth_cleanup_transmit( private_config, private_config->control_block );
// unlock the driver code
driver_unlock( &private_config->slock, &flags );
return 0;
}
/*
* Check if this packet is complete.
* Returns NULL on failure by any reason, and pointer
* to current nexthdr field in reassembled frame.
*
* It is called with locked fq, and caller must check that
* queue is eligible for reassembly i.e. it is not COMPLETE,
* the last and the first frames arrived and all the bits are here.
*/
static int ip6_frag_reasm(struct frag_queue *fq, struct sk_buff **skb_in,
struct net_device *dev)
{
struct sk_buff *fp, *head = fq->fragments;
int remove_fraghdr = 0;
int payload_len;
int nhoff;
fq_kill(fq);
BUG_TRAP(head != NULL);
BUG_TRAP(FRAG6_CB(head)->offset == 0);
/* Unfragmented part is taken from the first segment. */
payload_len = (head->data - head->nh.raw) - sizeof(struct ipv6hdr) + fq->len;
nhoff = head->h.raw - head->nh.raw;
if (payload_len > 65535) {
payload_len -= 8;
if (payload_len > 65535)
goto out_oversize;
remove_fraghdr = 1;
}
/* Head of list must not be cloned. */
if (skb_cloned(head) && pskb_expand_head(head, 0, 0, GFP_ATOMIC))
goto out_oom;
/* If the first fragment is fragmented itself, we split
* it to two chunks: the first with data and paged part
* and the second, holding only fragments. */
if (skb_shinfo(head)->frag_list) {
struct sk_buff *clone;
int i, plen = 0;
if ((clone = alloc_skb(0, GFP_ATOMIC)) == NULL)
goto out_oom;
clone->next = head->next;
head->next = clone;
skb_shinfo(clone)->frag_list = skb_shinfo(head)->frag_list;
skb_shinfo(head)->frag_list = NULL;
for (i=0; i<skb_shinfo(head)->nr_frags; i++)
plen += skb_shinfo(head)->frags[i].size;
clone->len = clone->data_len = head->data_len - plen;
head->data_len -= clone->len;
head->len -= clone->len;
clone->csum = 0;
clone->ip_summed = head->ip_summed;
atomic_add(clone->truesize, &ip6_frag_mem);
}
/* Normally we do not remove frag header from datagram, but
* we have to do this and to relocate header, when payload
* is > 65535-8. */
if (remove_fraghdr) {
nhoff = fq->nhoffset;
head->nh.raw[nhoff] = head->h.raw[0];
memmove(head->head+8, head->head, (head->data-head->head)-8);
head->mac.raw += 8;
head->nh.raw += 8;
} else {
((struct frag_hdr*)head->h.raw)->frag_off = 0;
}
skb_shinfo(head)->frag_list = head->next;
head->h.raw = head->data;
skb_push(head, head->data - head->nh.raw);
atomic_sub(head->truesize, &ip6_frag_mem);
for (fp=head->next; fp; fp = fp->next) {
head->data_len += fp->len;
head->len += fp->len;
if (head->ip_summed != fp->ip_summed)
head->ip_summed = CHECKSUM_NONE;
else if (head->ip_summed == CHECKSUM_HW)
head->csum = csum_add(head->csum, fp->csum);
head->truesize += fp->truesize;
atomic_sub(fp->truesize, &ip6_frag_mem);
}
head->next = NULL;
head->dev = dev;
head->stamp = fq->stamp;
head->nh.ipv6h->payload_len = ntohs(payload_len);
*skb_in = head;
/* Yes, and fold redundant checksum back. 8) */
if (head->ip_summed == CHECKSUM_HW)
head->csum = csum_partial(head->nh.raw, head->h.raw-head->nh.raw, head->csum);
IP6_INC_STATS_BH(Ip6ReasmOKs);
fq->fragments = NULL;
return nhoff;
out_oversize:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: payload len = %d\n", payload_len);
goto out_fail;
out_oom:
if (net_ratelimit())
printk(KERN_DEBUG "ip6_frag_reasm: no memory for reassembly\n");
out_fail:
IP6_INC_STATS_BH(Ip6ReasmFails);
return -1;
}
static int ip6_tnl_xmit2(struct sk_buff *skb,
struct net_device *dev,
__u8 dsfield,
struct flowi *fl,
int encap_limit,
__u32 *pmtu)
{
struct net *net = dev_net(dev);
struct ip6_tnl *t = netdev_priv(dev);
struct net_device_stats *stats = &t->dev->stats;
struct ipv6hdr *ipv6h = ipv6_hdr(skb);
struct ipv6_tel_txoption opt;
struct dst_entry *dst;
struct net_device *tdev;
int mtu;
unsigned int max_headroom = sizeof(struct ipv6hdr);
u8 proto;
int err = -1;
int pkt_len;
int check = 0;
if ((dst = ip6_tnl_dst_check(t)) != NULL)
dst_hold(dst);
else {
dst = ip6_route_output(net, NULL, fl);
if (dst->error || xfrm_lookup(net, &dst, fl, NULL, 0) < 0)
goto tx_err_link_failure;
}
tdev = dst->dev;
//printk("tunnel-->dst->dev name:%s, skb->len:%d, tunnel proto:%d\n", tdev->name, skb->len, t->parms.proto);
if (tdev == dev) {
stats->collisions++;
if (net_ratelimit())
printk(KERN_WARNING
"%s: Local routing loop detected!\n",
t->parms.name);
goto tx_err_dst_release;
}
mtu = dst_mtu(dst) - sizeof (*ipv6h);
if (encap_limit >= 0) {
max_headroom += 8;
mtu -= 8;
}
if (mtu < IPV6_MIN_MTU)
mtu = IPV6_MIN_MTU;
if (skb->dst)
skb->dst->ops->update_pmtu(skb->dst, mtu);
/*start by d00191326 rfc 2473:Dslite报文不进行V4分片而进行V6分片*/
if ((t->parms.proto == IPPROTO_IPIP && ip_hdr(skb)->frag_off&htons(IP_DF))
|| t->parms.proto == IPPROTO_IPV6){
check = 1;
}
/*end by d00191326 rfc 2473:Dslite报文不进行V4分片而进行V6分片*/
if (skb->len > mtu && check) {
*pmtu = mtu;
err = -EMSGSIZE;
goto tx_err_dst_release;
}
/*
* Okay, now see if we can stuff it in the buffer as-is.
*/
max_headroom += LL_RESERVED_SPACE(tdev);
if (skb_headroom(skb) < max_headroom || skb_shared(skb) ||
(skb_cloned(skb) && !skb_clone_writable(skb, 0))) {
struct sk_buff *new_skb;
if (!(new_skb = skb_realloc_headroom(skb, max_headroom)))
goto tx_err_dst_release;
if (skb->sk)
skb_set_owner_w(new_skb, skb->sk);
kfree_skb(skb);
skb = new_skb;
}
dst_release(skb->dst);
skb->dst = dst_clone(dst);
skb->transport_header = skb->network_header;
skb->local_df = 1;
proto = fl->proto;
if (encap_limit >= 0) {
init_tel_txopt(&opt, encap_limit);
ipv6_push_nfrag_opts(skb, &opt.ops, &proto, NULL);
}
skb_push(skb, sizeof(struct ipv6hdr));
skb_reset_network_header(skb);
ipv6h = ipv6_hdr(skb);
*(__be32*)ipv6h = fl->fl6_flowlabel | htonl(0x60000000);
dsfield = INET_ECN_encapsulate(dsfield, dsfield);
ipv6_change_dsfield(ipv6h, ~INET_ECN_MASK, dsfield);
ipv6h->hop_limit = t->parms.hop_limit;
ipv6h->nexthdr = proto;
ipv6_addr_copy(&ipv6h->saddr, &fl->fl6_src);
ipv6_addr_copy(&ipv6h->daddr, &fl->fl6_dst);
nf_reset(skb);
pkt_len = skb->len;
err = ip6_local_out(skb);
if (net_xmit_eval(err) == 0) {
stats->tx_bytes += pkt_len;
stats->tx_packets++;
} else {
stats->tx_errors++;
stats->tx_aborted_errors++;
}
ip6_tnl_dst_store(t, dst);
return 0;
tx_err_link_failure:
stats->tx_carrier_errors++;
dst_link_failure(skb);
tx_err_dst_release:
dst_release(dst);
return err;
}
static int ip6_fragment(struct sk_buff *skb, int (*output)(struct sk_buff *))
{
struct net_device *dev;
struct sk_buff *frag;
struct rt6_info *rt = (struct rt6_info*)skb->dst;
struct ipv6hdr *tmp_hdr;
struct frag_hdr *fh;
unsigned int mtu, hlen, left, len;
u32 frag_id = 0;
int ptr, offset = 0, err=0;
u8 *prevhdr, nexthdr = 0;
dev = rt->u.dst.dev;
hlen = ip6_find_1stfragopt(skb, &prevhdr);
nexthdr = *prevhdr;
mtu = dst_mtu(&rt->u.dst) - hlen - sizeof(struct frag_hdr);
if (skb_shinfo(skb)->frag_list) {
int first_len = skb_pagelen(skb);
if (first_len - hlen > mtu ||
((first_len - hlen) & 7) ||
skb_cloned(skb))
goto slow_path;
for (frag = skb_shinfo(skb)->frag_list; frag; frag = frag->next) {
/* Correct geometry. */
if (frag->len > mtu ||
((frag->len & 7) && frag->next) ||
skb_headroom(frag) < hlen)
goto slow_path;
/* Partially cloned skb? */
if (skb_shared(frag))
goto slow_path;
BUG_ON(frag->sk);
if (skb->sk) {
sock_hold(skb->sk);
frag->sk = skb->sk;
frag->destructor = sock_wfree;
skb->truesize -= frag->truesize;
}
}
err = 0;
offset = 0;
frag = skb_shinfo(skb)->frag_list;
skb_shinfo(skb)->frag_list = NULL;
/* BUILD HEADER */
tmp_hdr = kmalloc(hlen, GFP_ATOMIC);
if (!tmp_hdr) {
IP6_INC_STATS(IPSTATS_MIB_FRAGFAILS);
return -ENOMEM;
}
*prevhdr = NEXTHDR_FRAGMENT;
memcpy(tmp_hdr, skb->nh.raw, hlen);
__skb_pull(skb, hlen);
fh = (struct frag_hdr*)__skb_push(skb, sizeof(struct frag_hdr));
skb->nh.raw = __skb_push(skb, hlen);
memcpy(skb->nh.raw, tmp_hdr, hlen);
ipv6_select_ident(skb, fh);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(IP6_MF);
frag_id = fh->identification;
first_len = skb_pagelen(skb);
skb->data_len = first_len - skb_headlen(skb);
skb->len = first_len;
skb->nh.ipv6h->payload_len = htons(first_len - sizeof(struct ipv6hdr));
for (;;) {
/* Prepare header of the next frame,
* before previous one went down. */
if (frag) {
frag->ip_summed = CHECKSUM_NONE;
frag->h.raw = frag->data;
fh = (struct frag_hdr*)__skb_push(frag, sizeof(struct frag_hdr));
frag->nh.raw = __skb_push(frag, hlen);
memcpy(frag->nh.raw, tmp_hdr, hlen);
offset += skb->len - hlen - sizeof(struct frag_hdr);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(offset);
if (frag->next != NULL)
fh->frag_off |= htons(IP6_MF);
fh->identification = frag_id;
frag->nh.ipv6h->payload_len = htons(frag->len - sizeof(struct ipv6hdr));
ip6_copy_metadata(frag, skb);
}
err = output(skb);
if (err || !frag)
break;
skb = frag;
frag = skb->next;
skb->next = NULL;
}
if (tmp_hdr)
kfree(tmp_hdr);
if (err == 0) {
IP6_INC_STATS(IPSTATS_MIB_FRAGOKS);
return 0;
}
while (frag) {
skb = frag->next;
kfree_skb(frag);
frag = skb;
}
IP6_INC_STATS(IPSTATS_MIB_FRAGFAILS);
return err;
}
slow_path:
left = skb->len - hlen; /* Space per frame */
ptr = hlen; /* Where to start from */
/*
* Fragment the datagram.
*/
*prevhdr = NEXTHDR_FRAGMENT;
/*
* Keep copying data until we run out.
*/
while(left > 0) {
len = left;
/* IF: it doesn't fit, use 'mtu' - the data space left */
if (len > mtu)
len = mtu;
/* IF: we are not sending upto and including the packet end
then align the next start on an eight byte boundary */
if (len < left) {
len &= ~7;
}
/*
* Allocate buffer.
*/
if ((frag = alloc_skb(len+hlen+sizeof(struct frag_hdr)+LL_RESERVED_SPACE(rt->u.dst.dev), GFP_ATOMIC)) == NULL) {
NETDEBUG(printk(KERN_INFO "IPv6: frag: no memory for new fragment!\n"));
IP6_INC_STATS(IPSTATS_MIB_FRAGFAILS);
err = -ENOMEM;
goto fail;
}
/*
* Set up data on packet
*/
ip6_copy_metadata(frag, skb);
skb_reserve(frag, LL_RESERVED_SPACE(rt->u.dst.dev));
skb_put(frag, len + hlen + sizeof(struct frag_hdr));
frag->nh.raw = frag->data;
fh = (struct frag_hdr*)(frag->data + hlen);
frag->h.raw = frag->data + hlen + sizeof(struct frag_hdr);
/*
* Charge the memory for the fragment to any owner
* it might possess
*/
if (skb->sk)
skb_set_owner_w(frag, skb->sk);
/*
* Copy the packet header into the new buffer.
*/
memcpy(frag->nh.raw, skb->data, hlen);
/*
* Build fragment header.
*/
fh->nexthdr = nexthdr;
fh->reserved = 0;
if (frag_id) {
ipv6_select_ident(skb, fh);
frag_id = fh->identification;
} else
fh->identification = frag_id;
/*
* Copy a block of the IP datagram.
*/
if (skb_copy_bits(skb, ptr, frag->h.raw, len))
BUG();
left -= len;
fh->frag_off = htons(offset);
if (left > 0)
fh->frag_off |= htons(IP6_MF);
frag->nh.ipv6h->payload_len = htons(frag->len - sizeof(struct ipv6hdr));
ptr += len;
offset += len;
/*
* Put this fragment into the sending queue.
*/
IP6_INC_STATS(IPSTATS_MIB_FRAGCREATES);
err = output(frag);
if (err)
goto fail;
}
kfree_skb(skb);
IP6_INC_STATS(IPSTATS_MIB_FRAGOKS);
return err;
fail:
kfree_skb(skb);
IP6_INC_STATS(IPSTATS_MIB_FRAGFAILS);
return err;
}
static int tcf_nat(struct sk_buff *skb, struct tc_action *a,
struct tcf_result *res)
{
struct tcf_nat *p = a->priv;
struct iphdr *iph;
__be32 old_addr;
__be32 new_addr;
__be32 mask;
__be32 addr;
int egress;
int action;
int ihl;
spin_lock(&p->tcf_lock);
p->tcf_tm.lastuse = jiffies;
old_addr = p->old_addr;
new_addr = p->new_addr;
mask = p->mask;
egress = p->flags & TCA_NAT_FLAG_EGRESS;
action = p->tcf_action;
p->tcf_bstats.bytes += qdisc_pkt_len(skb);
p->tcf_bstats.packets++;
spin_unlock(&p->tcf_lock);
if (unlikely(action == TC_ACT_SHOT))
goto drop;
if (!pskb_may_pull(skb, sizeof(*iph)))
goto drop;
iph = ip_hdr(skb);
if (egress)
addr = iph->saddr;
else
addr = iph->daddr;
if (!((old_addr ^ addr) & mask)) {
if (skb_cloned(skb) &&
!skb_clone_writable(skb, sizeof(*iph)) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
new_addr &= mask;
new_addr |= addr & ~mask;
/* Rewrite IP header */
iph = ip_hdr(skb);
if (egress)
iph->saddr = new_addr;
else
iph->daddr = new_addr;
csum_replace4(&iph->check, addr, new_addr);
} else if ((iph->frag_off & htons(IP_OFFSET)) ||
iph->protocol != IPPROTO_ICMP) {
goto out;
}
ihl = iph->ihl * 4;
/* It would be nice to share code with stateful NAT. */
switch (iph->frag_off & htons(IP_OFFSET) ? 0 : iph->protocol) {
case IPPROTO_TCP:
{
struct tcphdr *tcph;
if (!pskb_may_pull(skb, ihl + sizeof(*tcph)) ||
(skb_cloned(skb) &&
!skb_clone_writable(skb, ihl + sizeof(*tcph)) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC)))
goto drop;
tcph = (void *)(skb_network_header(skb) + ihl);
inet_proto_csum_replace4(&tcph->check, skb, addr, new_addr, 1);
break;
}
case IPPROTO_UDP:
{
struct udphdr *udph;
if (!pskb_may_pull(skb, ihl + sizeof(*udph)) ||
(skb_cloned(skb) &&
!skb_clone_writable(skb, ihl + sizeof(*udph)) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC)))
goto drop;
udph = (void *)(skb_network_header(skb) + ihl);
if (udph->check || skb->ip_summed == CHECKSUM_PARTIAL) {
inet_proto_csum_replace4(&udph->check, skb, addr,
new_addr, 1);
if (!udph->check)
udph->check = CSUM_MANGLED_0;
}
break;
}
case IPPROTO_ICMP:
{
struct icmphdr *icmph;
if (!pskb_may_pull(skb, ihl + sizeof(*icmph)))
goto drop;
icmph = (void *)(skb_network_header(skb) + ihl);
if ((icmph->type != ICMP_DEST_UNREACH) &&
(icmph->type != ICMP_TIME_EXCEEDED) &&
(icmph->type != ICMP_PARAMETERPROB))
break;
if (!pskb_may_pull(skb, ihl + sizeof(*icmph) + sizeof(*iph)))
goto drop;
icmph = (void *)(skb_network_header(skb) + ihl);
iph = (void *)(icmph + 1);
if (egress)
addr = iph->daddr;
else
addr = iph->saddr;
if ((old_addr ^ addr) & mask)
break;
if (skb_cloned(skb) &&
!skb_clone_writable(skb,
ihl + sizeof(*icmph) + sizeof(*iph)) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
icmph = (void *)(skb_network_header(skb) + ihl);
iph = (void *)(icmph + 1);
new_addr &= mask;
new_addr |= addr & ~mask;
/* XXX Fix up the inner checksums. */
if (egress)
iph->daddr = new_addr;
else
iph->saddr = new_addr;
inet_proto_csum_replace4(&icmph->checksum, skb, addr, new_addr,
0);
break;
}
default:
break;
}
out:
return action;
drop:
spin_lock(&p->tcf_lock);
p->tcf_qstats.drops++;
spin_unlock(&p->tcf_lock);
return TC_ACT_SHOT;
}
static int ah6_output(struct xfrm_state *x, struct sk_buff *skb)
{
int err;
int extlen;
struct ipv6hdr *top_iph;
struct ip_auth_hdr *ah;
struct ah_data *ahp;
u8 nexthdr;
char tmp_base[8];
struct {
struct in6_addr daddr;
char hdrs[0];
} *tmp_ext;
#if defined (CONFIG_OCTEON_NATIVE_IPSEC)
if ((skb_is_nonlinear(skb) || skb_cloned(skb)) &&
skb_linearize(skb, GFP_ATOMIC) != 0) {
err = -ENOMEM;
printk("Unable to linearize memory %d\n", skb->len);
goto error;
}
#endif
top_iph = (struct ipv6hdr *)skb->data;
top_iph->payload_len = htons(skb->len - sizeof(*top_iph));
nexthdr = *skb->nh.raw;
*skb->nh.raw = IPPROTO_AH;
/* When there are no extension headers, we only need to save the first
* 8 bytes of the base IP header.
*/
memcpy(tmp_base, top_iph, sizeof(tmp_base));
tmp_ext = NULL;
extlen = skb->h.raw - (unsigned char *)(top_iph + 1);
if (extlen) {
extlen += sizeof(*tmp_ext);
tmp_ext = kmalloc(extlen, GFP_ATOMIC);
if (!tmp_ext) {
err = -ENOMEM;
goto error;
}
memcpy(tmp_ext, &top_iph->daddr, extlen);
err = ipv6_clear_mutable_options(top_iph,
extlen - sizeof(*tmp_ext) +
sizeof(*top_iph));
if (err)
goto error_free_iph;
}
ah = (struct ip_auth_hdr *)skb->h.raw;
ah->nexthdr = nexthdr;
top_iph->priority = 0;
top_iph->flow_lbl[0] = 0;
top_iph->flow_lbl[1] = 0;
top_iph->flow_lbl[2] = 0;
top_iph->hop_limit = 0;
ahp = x->data;
ah->hdrlen = (XFRM_ALIGN8(sizeof(struct ipv6_auth_hdr) +
ahp->icv_trunc_len) >> 2) - 2;
ah->reserved = 0;
ah->spi = x->id.spi;
ah->seq_no = htonl(++x->replay.oseq);
#if defined (CONFIG_OCTEON_NATIVE_IPSEC)
cavium_process_ah_pkt(ahp, skb, ah->auth_data);
#else
ahp->icv(ahp, skb, ah->auth_data);
#endif
err = 0;
memcpy(top_iph, tmp_base, sizeof(tmp_base));
if (tmp_ext) {
memcpy(&top_iph->daddr, tmp_ext, extlen);
error_free_iph:
kfree(tmp_ext);
}
error:
return err;
}
int ip6_fragment(struct net *net, struct sock *sk, struct sk_buff *skb,
int (*output)(struct net *, struct sock *, struct sk_buff *))
{
struct sk_buff *frag;
struct rt6_info *rt = (struct rt6_info *)skb_dst(skb);
struct ipv6_pinfo *np = skb->sk && !dev_recursion_level() ?
inet6_sk(skb->sk) : NULL;
struct ipv6hdr *tmp_hdr;
struct frag_hdr *fh;
unsigned int mtu, hlen, left, len;
int hroom, troom;
__be32 frag_id;
int ptr, offset = 0, err = 0;
u8 *prevhdr, nexthdr = 0;
hlen = ip6_find_1stfragopt(skb, &prevhdr);
nexthdr = *prevhdr;
mtu = ip6_skb_dst_mtu(skb);
/* We must not fragment if the socket is set to force MTU discovery
* or if the skb it not generated by a local socket.
*/
if (unlikely(!skb->ignore_df && skb->len > mtu))
goto fail_toobig;
if (IP6CB(skb)->frag_max_size) {
if (IP6CB(skb)->frag_max_size > mtu)
goto fail_toobig;
/* don't send fragments larger than what we received */
mtu = IP6CB(skb)->frag_max_size;
if (mtu < IPV6_MIN_MTU)
mtu = IPV6_MIN_MTU;
}
if (np && np->frag_size < mtu) {
if (np->frag_size)
mtu = np->frag_size;
}
if (mtu < hlen + sizeof(struct frag_hdr) + 8)
goto fail_toobig;
mtu -= hlen + sizeof(struct frag_hdr);
frag_id = ipv6_select_ident(net, &ipv6_hdr(skb)->daddr,
&ipv6_hdr(skb)->saddr);
if (skb->ip_summed == CHECKSUM_PARTIAL &&
(err = skb_checksum_help(skb)))
goto fail;
hroom = LL_RESERVED_SPACE(rt->dst.dev);
if (skb_has_frag_list(skb)) {
int first_len = skb_pagelen(skb);
struct sk_buff *frag2;
if (first_len - hlen > mtu ||
((first_len - hlen) & 7) ||
skb_cloned(skb) ||
skb_headroom(skb) < (hroom + sizeof(struct frag_hdr)))
goto slow_path;
skb_walk_frags(skb, frag) {
/* Correct geometry. */
if (frag->len > mtu ||
((frag->len & 7) && frag->next) ||
skb_headroom(frag) < (hlen + hroom + sizeof(struct frag_hdr)))
goto slow_path_clean;
/* Partially cloned skb? */
if (skb_shared(frag))
goto slow_path_clean;
BUG_ON(frag->sk);
if (skb->sk) {
frag->sk = skb->sk;
frag->destructor = sock_wfree;
}
skb->truesize -= frag->truesize;
}
err = 0;
offset = 0;
/* BUILD HEADER */
*prevhdr = NEXTHDR_FRAGMENT;
tmp_hdr = kmemdup(skb_network_header(skb), hlen, GFP_ATOMIC);
if (!tmp_hdr) {
IP6_INC_STATS(net, ip6_dst_idev(skb_dst(skb)),
IPSTATS_MIB_FRAGFAILS);
err = -ENOMEM;
goto fail;
}
frag = skb_shinfo(skb)->frag_list;
skb_frag_list_init(skb);
__skb_pull(skb, hlen);
fh = (struct frag_hdr *)__skb_push(skb, sizeof(struct frag_hdr));
__skb_push(skb, hlen);
skb_reset_network_header(skb);
memcpy(skb_network_header(skb), tmp_hdr, hlen);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(IP6_MF);
fh->identification = frag_id;
first_len = skb_pagelen(skb);
skb->data_len = first_len - skb_headlen(skb);
skb->len = first_len;
ipv6_hdr(skb)->payload_len = htons(first_len -
sizeof(struct ipv6hdr));
dst_hold(&rt->dst);
for (;;) {
/* Prepare header of the next frame,
* before previous one went down. */
if (frag) {
frag->ip_summed = CHECKSUM_NONE;
skb_reset_transport_header(frag);
fh = (struct frag_hdr *)__skb_push(frag, sizeof(struct frag_hdr));
__skb_push(frag, hlen);
skb_reset_network_header(frag);
memcpy(skb_network_header(frag), tmp_hdr,
hlen);
offset += skb->len - hlen - sizeof(struct frag_hdr);
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->frag_off = htons(offset);
if (frag->next)
fh->frag_off |= htons(IP6_MF);
fh->identification = frag_id;
ipv6_hdr(frag)->payload_len =
htons(frag->len -
sizeof(struct ipv6hdr));
ip6_copy_metadata(frag, skb);
}
err = output(net, sk, skb);
if (!err)
IP6_INC_STATS(net, ip6_dst_idev(&rt->dst),
IPSTATS_MIB_FRAGCREATES);
if (err || !frag)
break;
skb = frag;
frag = skb->next;
skb->next = NULL;
}
kfree(tmp_hdr);
if (err == 0) {
IP6_INC_STATS(net, ip6_dst_idev(&rt->dst),
IPSTATS_MIB_FRAGOKS);
ip6_rt_put(rt);
return 0;
}
kfree_skb_list(frag);
IP6_INC_STATS(net, ip6_dst_idev(&rt->dst),
IPSTATS_MIB_FRAGFAILS);
ip6_rt_put(rt);
return err;
slow_path_clean:
skb_walk_frags(skb, frag2) {
if (frag2 == frag)
break;
frag2->sk = NULL;
frag2->destructor = NULL;
skb->truesize += frag2->truesize;
}
}
slow_path:
left = skb->len - hlen; /* Space per frame */
ptr = hlen; /* Where to start from */
/*
* Fragment the datagram.
*/
*prevhdr = NEXTHDR_FRAGMENT;
troom = rt->dst.dev->needed_tailroom;
/*
* Keep copying data until we run out.
*/
while (left > 0) {
len = left;
/* IF: it doesn't fit, use 'mtu' - the data space left */
if (len > mtu)
len = mtu;
/* IF: we are not sending up to and including the packet end
then align the next start on an eight byte boundary */
if (len < left) {
len &= ~7;
}
/* Allocate buffer */
frag = alloc_skb(len + hlen + sizeof(struct frag_hdr) +
hroom + troom, GFP_ATOMIC);
if (!frag) {
IP6_INC_STATS(net, ip6_dst_idev(skb_dst(skb)),
IPSTATS_MIB_FRAGFAILS);
err = -ENOMEM;
goto fail;
}
/*
* Set up data on packet
*/
ip6_copy_metadata(frag, skb);
skb_reserve(frag, hroom);
skb_put(frag, len + hlen + sizeof(struct frag_hdr));
skb_reset_network_header(frag);
fh = (struct frag_hdr *)(skb_network_header(frag) + hlen);
frag->transport_header = (frag->network_header + hlen +
sizeof(struct frag_hdr));
/*
* Charge the memory for the fragment to any owner
* it might possess
*/
if (skb->sk)
skb_set_owner_w(frag, skb->sk);
/*
* Copy the packet header into the new buffer.
*/
skb_copy_from_linear_data(skb, skb_network_header(frag), hlen);
/*
* Build fragment header.
*/
fh->nexthdr = nexthdr;
fh->reserved = 0;
fh->identification = frag_id;
/*
* Copy a block of the IP datagram.
*/
BUG_ON(skb_copy_bits(skb, ptr, skb_transport_header(frag),
len));
left -= len;
fh->frag_off = htons(offset);
if (left > 0)
fh->frag_off |= htons(IP6_MF);
ipv6_hdr(frag)->payload_len = htons(frag->len -
sizeof(struct ipv6hdr));
ptr += len;
offset += len;
/*
* Put this fragment into the sending queue.
*/
err = output(net, sk, frag);
if (err)
goto fail;
IP6_INC_STATS(net, ip6_dst_idev(skb_dst(skb)),
IPSTATS_MIB_FRAGCREATES);
}
IP6_INC_STATS(net, ip6_dst_idev(skb_dst(skb)),
IPSTATS_MIB_FRAGOKS);
consume_skb(skb);
return err;
fail_toobig:
if (skb->sk && dst_allfrag(skb_dst(skb)))
sk_nocaps_add(skb->sk, NETIF_F_GSO_MASK);
skb->dev = skb_dst(skb)->dev;
icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu);
err = -EMSGSIZE;
fail:
IP6_INC_STATS(net, ip6_dst_idev(skb_dst(skb)),
IPSTATS_MIB_FRAGFAILS);
kfree_skb(skb);
return err;
}
static int ipip6_tunnel_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct ip_tunnel *tunnel = netdev_priv(dev);
struct net_device_stats *stats = &tunnel->stat;
struct iphdr *tiph = &tunnel->parms.iph;
struct ipv6hdr *iph6 = ipv6_hdr(skb);
u8 tos = tunnel->parms.iph.tos;
struct rtable *rt; /* Route to the other host */
struct net_device *tdev; /* Device to other host */
struct iphdr *iph; /* Our new IP header */
int max_headroom; /* The extra header space needed */
__be32 dst = tiph->daddr;
int mtu;
struct in6_addr *addr6;
int addr_type;
if (tunnel->recursion++) {
tunnel->stat.collisions++;
goto tx_error;
}
if (skb->protocol != htons(ETH_P_IPV6))
goto tx_error;
if (!dst)
dst = try_6to4(&iph6->daddr);
if (!dst) {
struct neighbour *neigh = NULL;
if (skb->dst)
neigh = skb->dst->neighbour;
if (neigh == NULL) {
if (net_ratelimit())
printk(KERN_DEBUG "sit: nexthop == NULL\n");
goto tx_error;
}
addr6 = (struct in6_addr*)&neigh->primary_key;
addr_type = ipv6_addr_type(addr6);
if (addr_type == IPV6_ADDR_ANY) {
addr6 = &ipv6_hdr(skb)->daddr;
addr_type = ipv6_addr_type(addr6);
}
if ((addr_type & IPV6_ADDR_COMPATv4) == 0)
goto tx_error_icmp;
dst = addr6->s6_addr32[3];
}
{
struct flowi fl = { .nl_u = { .ip4_u =
{ .daddr = dst,
.saddr = tiph->saddr,
.tos = RT_TOS(tos) } },
.oif = tunnel->parms.link,
.proto = IPPROTO_IPV6 };
if (ip_route_output_key(&rt, &fl)) {
tunnel->stat.tx_carrier_errors++;
goto tx_error_icmp;
}
}
if (rt->rt_type != RTN_UNICAST) {
ip_rt_put(rt);
tunnel->stat.tx_carrier_errors++;
goto tx_error_icmp;
}
tdev = rt->u.dst.dev;
if (tdev == dev) {
ip_rt_put(rt);
tunnel->stat.collisions++;
goto tx_error;
}
if (tiph->frag_off)
mtu = dst_mtu(&rt->u.dst) - sizeof(struct iphdr);
else
mtu = skb->dst ? dst_mtu(skb->dst) : dev->mtu;
if (mtu < 68) {
tunnel->stat.collisions++;
ip_rt_put(rt);
goto tx_error;
}
if (mtu < IPV6_MIN_MTU)
mtu = IPV6_MIN_MTU;
if (tunnel->parms.iph.daddr && skb->dst)
skb->dst->ops->update_pmtu(skb->dst, mtu);
if (skb->len > mtu) {
icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu, dev);
ip_rt_put(rt);
goto tx_error;
}
if (tunnel->err_count > 0) {
if (jiffies - tunnel->err_time < IPTUNNEL_ERR_TIMEO) {
tunnel->err_count--;
dst_link_failure(skb);
} else
tunnel->err_count = 0;
}
/*
* Okay, now see if we can stuff it in the buffer as-is.
*/
max_headroom = LL_RESERVED_SPACE(tdev)+sizeof(struct iphdr);
if (skb_headroom(skb) < max_headroom || skb_shared(skb) ||
(skb_cloned(skb) && !skb_clone_writable(skb, 0))) {
struct sk_buff *new_skb = skb_realloc_headroom(skb, max_headroom);
if (!new_skb) {
ip_rt_put(rt);
stats->tx_dropped++;
dev_kfree_skb(skb);
tunnel->recursion--;
return 0;
}
if (skb->sk)
skb_set_owner_w(new_skb, skb->sk);
dev_kfree_skb(skb);
skb = new_skb;
iph6 = ipv6_hdr(skb);
}
skb->transport_header = skb->network_header;
skb_push(skb, sizeof(struct iphdr));
skb_reset_network_header(skb);
memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
IPCB(skb)->flags = 0;
dst_release(skb->dst);
skb->dst = &rt->u.dst;
/*
* Push down and install the IPIP header.
*/
iph = ip_hdr(skb);
iph->version = 4;
iph->ihl = sizeof(struct iphdr)>>2;
if (mtu > IPV6_MIN_MTU)
iph->frag_off = htons(IP_DF);
else
iph->frag_off = 0;
iph->protocol = IPPROTO_IPV6;
iph->tos = INET_ECN_encapsulate(tos, ipv6_get_dsfield(iph6));
iph->daddr = rt->rt_dst;
iph->saddr = rt->rt_src;
if ((iph->ttl = tiph->ttl) == 0)
iph->ttl = iph6->hop_limit;
nf_reset(skb);
IPTUNNEL_XMIT();
tunnel->recursion--;
return 0;
tx_error_icmp:
dst_link_failure(skb);
tx_error:
stats->tx_errors++;
dev_kfree_skb(skb);
tunnel->recursion--;
return 0;
}
static int ah_input(struct xfrm_state *x, struct sk_buff *skb)
{
int ah_hlen;
int ihl;
int err = -EINVAL;
struct iphdr *iph;
struct ip_auth_hdr *ah;
struct ah_data *ahp;
char work_buf[60];
if (!pskb_may_pull(skb, sizeof(struct ip_auth_hdr)))
goto out;
ah = (struct ip_auth_hdr*)skb->data;
ahp = x->data;
ah_hlen = (ah->hdrlen + 2) << 2;
if (ah_hlen != XFRM_ALIGN8(sizeof(struct ip_auth_hdr) + ahp->icv_full_len) &&
ah_hlen != XFRM_ALIGN8(sizeof(struct ip_auth_hdr) + ahp->icv_trunc_len))
goto out;
if (!pskb_may_pull(skb, ah_hlen))
goto out;
/* We are going to _remove_ AH header to keep sockets happy,
* so... Later this can change. */
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto out;
skb->ip_summed = CHECKSUM_NONE;
ah = (struct ip_auth_hdr*)skb->data;
iph = skb->nh.iph;
ihl = skb->data - skb->nh.raw;
memcpy(work_buf, iph, ihl);
iph->ttl = 0;
iph->tos = 0;
iph->frag_off = 0;
iph->check = 0;
if (ihl > sizeof(*iph)) {
__be32 dummy;
if (ip_clear_mutable_options(iph, &dummy))
goto out;
}
{
u8 auth_data[MAX_AH_AUTH_LEN];
memcpy(auth_data, ah->auth_data, ahp->icv_trunc_len);
skb_push(skb, ihl);
err = ah_mac_digest(ahp, skb, ah->auth_data);
if (err)
goto out;
err = -EINVAL;
if (memcmp(ahp->work_icv, auth_data, ahp->icv_trunc_len)) {
x->stats.integrity_failed++;
goto out;
}
}
((struct iphdr*)work_buf)->protocol = ah->nexthdr;
skb->h.raw = memcpy(skb->nh.raw += ah_hlen, work_buf, ihl);
__skb_pull(skb, ah_hlen + ihl);
return 0;
out:
return err;
}
static int ah6_input(struct xfrm_state *x, struct sk_buff *skb)
{
/*
* Before process AH
* [IPv6][Ext1][Ext2][AH][Dest][Payload]
* |<-------------->| hdr_len
*
* To erase AH:
* Keeping copy of cleared headers. After AH processing,
* Moving the pointer of skb->network_header by using skb_pull as long
* as AH header length. Then copy back the copy as long as hdr_len
* If destination header following AH exists, copy it into after [Ext2].
*
* |<>|[IPv6][Ext1][Ext2][Dest][Payload]
* There is offset of AH before IPv6 header after the process.
*/
struct ip_auth_hdr *ah;
struct ipv6hdr *ip6h;
struct ah_data *ahp;
unsigned char *tmp_hdr = NULL;
u16 hdr_len;
u16 ah_hlen;
int nexthdr;
int err = -EINVAL;
if (!pskb_may_pull(skb, sizeof(struct ip_auth_hdr)))
goto out;
/* We are going to _remove_ AH header to keep sockets happy,
* so... Later this can change. */
if (skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto out;
skb->ip_summed = CHECKSUM_NONE;
hdr_len = skb->data - skb_network_header(skb);
ah = (struct ip_auth_hdr *)skb->data;
ahp = x->data;
nexthdr = ah->nexthdr;
ah_hlen = (ah->hdrlen + 2) << 2;
if (ah_hlen != XFRM_ALIGN8(sizeof(*ah) + ahp->icv_full_len) &&
ah_hlen != XFRM_ALIGN8(sizeof(*ah) + ahp->icv_trunc_len))
goto out;
if (!pskb_may_pull(skb, ah_hlen))
goto out;
tmp_hdr = kmemdup(skb_network_header(skb), hdr_len, GFP_ATOMIC);
if (!tmp_hdr)
goto out;
ip6h = ipv6_hdr(skb);
if (ipv6_clear_mutable_options(ip6h, hdr_len, XFRM_POLICY_IN))
goto free_out;
ip6h->priority = 0;
ip6h->flow_lbl[0] = 0;
ip6h->flow_lbl[1] = 0;
ip6h->flow_lbl[2] = 0;
ip6h->hop_limit = 0;
spin_lock(&x->lock);
{
u8 auth_data[MAX_AH_AUTH_LEN];
memcpy(auth_data, ah->auth_data, ahp->icv_trunc_len);
memset(ah->auth_data, 0, ahp->icv_trunc_len);
skb_push(skb, hdr_len);
err = ah_mac_digest(ahp, skb, ah->auth_data);
if (err)
goto unlock;
if (memcmp(ahp->work_icv, auth_data, ahp->icv_trunc_len))
err = -EBADMSG;
}
unlock:
spin_unlock(&x->lock);
if (err)
goto free_out;
skb->network_header += ah_hlen;
memcpy(skb_network_header(skb), tmp_hdr, hdr_len);
skb->transport_header = skb->network_header;
__skb_pull(skb, ah_hlen + hdr_len);
kfree(tmp_hdr);
return nexthdr;
free_out:
kfree(tmp_hdr);
out:
return err;
}
/*
* Check if this packet is complete.
* Returns NULL on failure by any reason, and pointer
* to current nexthdr field in reassembled frame.
*
* It is called with locked fq, and caller must check that
* queue is eligible for reassembly i.e. it is not COMPLETE,
* the last and the first frames arrived and all the bits are here.
*/
static struct sk_buff *
nf_ct_frag6_reasm(struct nf_ct_frag6_queue *fq, struct net_device *dev)
{
struct sk_buff *fp, *op, *head = fq->fragments;
int payload_len;
fq_kill(fq);
BUG_TRAP(head != NULL);
BUG_TRAP(NFCT_FRAG6_CB(head)->offset == 0);
/* Unfragmented part is taken from the first segment. */
payload_len = (head->data - head->nh.raw) - sizeof(struct ipv6hdr) + fq->len - sizeof(struct frag_hdr);
if (payload_len > IPV6_MAXPLEN) {
DEBUGP("payload len is too large.\n");
goto out_oversize;
}
/* Head of list must not be cloned. */
if (skb_cloned(head) && pskb_expand_head(head, 0, 0, GFP_ATOMIC)) {
DEBUGP("skb is cloned but can't expand head");
goto out_oom;
}
/* If the first fragment is fragmented itself, we split
* it to two chunks: the first with data and paged part
* and the second, holding only fragments. */
if (skb_shinfo(head)->frag_list) {
struct sk_buff *clone;
int i, plen = 0;
if ((clone = alloc_skb(0, GFP_ATOMIC)) == NULL) {
DEBUGP("Can't alloc skb\n");
goto out_oom;
}
clone->next = head->next;
head->next = clone;
skb_shinfo(clone)->frag_list = skb_shinfo(head)->frag_list;
skb_shinfo(head)->frag_list = NULL;
for (i=0; i<skb_shinfo(head)->nr_frags; i++)
plen += skb_shinfo(head)->frags[i].size;
clone->len = clone->data_len = head->data_len - plen;
head->data_len -= clone->len;
head->len -= clone->len;
clone->csum = 0;
clone->ip_summed = head->ip_summed;
NFCT_FRAG6_CB(clone)->orig = NULL;
atomic_add(clone->truesize, &nf_ct_frag6_mem);
}
/* We have to remove fragment header from datagram and to relocate
* header in order to calculate ICV correctly. */
head->nh.raw[fq->nhoffset] = head->h.raw[0];
memmove(head->head + sizeof(struct frag_hdr), head->head,
(head->data - head->head) - sizeof(struct frag_hdr));
head->mac.raw += sizeof(struct frag_hdr);
head->nh.raw += sizeof(struct frag_hdr);
skb_shinfo(head)->frag_list = head->next;
head->h.raw = head->data;
skb_push(head, head->data - head->nh.raw);
atomic_sub(head->truesize, &nf_ct_frag6_mem);
for (fp=head->next; fp; fp = fp->next) {
head->data_len += fp->len;
head->len += fp->len;
if (head->ip_summed != fp->ip_summed)
head->ip_summed = CHECKSUM_NONE;
else if (head->ip_summed == CHECKSUM_HW)
head->csum = csum_add(head->csum, fp->csum);
head->truesize += fp->truesize;
atomic_sub(fp->truesize, &nf_ct_frag6_mem);
}
head->next = NULL;
head->dev = dev;
skb_set_timestamp(head, &fq->stamp);
head->nh.ipv6h->payload_len = htons(payload_len);
/* Yes, and fold redundant checksum back. 8) */
if (head->ip_summed == CHECKSUM_HW)
head->csum = csum_partial(head->nh.raw, head->h.raw-head->nh.raw, head->csum);
fq->fragments = NULL;
/* all original skbs are linked into the NFCT_FRAG6_CB(head).orig */
fp = skb_shinfo(head)->frag_list;
if (NFCT_FRAG6_CB(fp)->orig == NULL)
/* at above code, head skb is divided into two skbs. */
fp = fp->next;
op = NFCT_FRAG6_CB(head)->orig;
for (; fp; fp = fp->next) {
struct sk_buff *orig = NFCT_FRAG6_CB(fp)->orig;
op->next = orig;
op = orig;
NFCT_FRAG6_CB(fp)->orig = NULL;
}
return head;
out_oversize:
if (net_ratelimit())
printk(KERN_DEBUG "nf_ct_frag6_reasm: payload len = %d\n", payload_len);
goto out_fail;
out_oom:
if (net_ratelimit())
printk(KERN_DEBUG "nf_ct_frag6_reasm: no memory for reassembly\n");
out_fail:
return NULL;
}
/* If it's a keepalive packet, then just eat it.
* If it's an encapsulated packet, then pass it to the
* IPsec xfrm input.
* Returns 0 if skb passed to xfrm or was dropped.
* Returns >0 if skb should be passed to UDP.
* Returns <0 if skb should be resubmitted (-ret is protocol)
*/
int xfrm4_udp_encap_rcv(struct sock *sk, struct sk_buff *skb)
{
struct udp_sock *up = udp_sk(sk);
struct udphdr *uh;
struct iphdr *iph;
int iphlen, len;
__u8 *udpdata;
__be32 *udpdata32;
__u16 encap_type = up->encap_type;
/* if this is not encapsulated socket, then just return now */
if (!encap_type)
return 1;
/* If this is a paged skb, make sure we pull up
* whatever data we need to look at. */
len = skb->len - sizeof(struct udphdr);
if (!pskb_may_pull(skb, sizeof(struct udphdr) + min(len, 8)))
return 1;
/* Now we can get the pointers */
uh = udp_hdr(skb);
udpdata = (__u8 *)uh + sizeof(struct udphdr);
udpdata32 = (__be32 *)udpdata;
switch (encap_type) {
default:
case UDP_ENCAP_ESPINUDP:
/* Check if this is a keepalive packet. If so, eat it. */
if (len == 1 && udpdata[0] == 0xff) {
goto drop;
} else if (len > sizeof(struct ip_esp_hdr) && udpdata32[0] != 0) {
/* ESP Packet without Non-ESP header */
len = sizeof(struct udphdr);
} else
/* Must be an IKE packet.. pass it through */
return 1;
break;
case UDP_ENCAP_ESPINUDP_NON_IKE:
/* Check if this is a keepalive packet. If so, eat it. */
if (len == 1 && udpdata[0] == 0xff) {
goto drop;
} else if (len > 2 * sizeof(u32) + sizeof(struct ip_esp_hdr) &&
udpdata32[0] == 0 && udpdata32[1] == 0) {
/* ESP Packet with Non-IKE marker */
len = sizeof(struct udphdr) + 2 * sizeof(u32);
} else
/* Must be an IKE packet.. pass it through */
return 1;
break;
}
/* At this point we are sure that this is an ESPinUDP packet,
* so we need to remove 'len' bytes from the packet (the UDP
* header and optional ESP marker bytes) and then modify the
* protocol to ESP, and then call into the transform receiver.
*/
if (skb_cloned(skb) && pskb_expand_head(skb, 0, 0, GFP_ATOMIC))
goto drop;
/* Now we can update and verify the packet length... */
iph = ip_hdr(skb);
iphlen = iph->ihl << 2;
iph->tot_len = htons(ntohs(iph->tot_len) - len);
if (skb->len < iphlen + len) {
/* packet is too small!?! */
goto drop;
}
/* pull the data buffer up to the ESP header and set the
* transport header to point to ESP. Keep UDP on the stack
* for later.
*/
__skb_pull(skb, len);
skb_reset_transport_header(skb);
/* process ESP */
return xfrm4_rcv_encap(skb, IPPROTO_ESP, 0, encap_type);
drop:
kfree_skb(skb);
return 0;
}
static int netdev_send(struct vport *vport, struct sk_buff *skb)
{
struct netdev_vport *netdev_vport = netdev_vport_priv(vport);
int mtu = netdev_vport->dev->mtu;
int len;
if (unlikely(packet_length(skb) > mtu && !skb_is_gso(skb))) {
net_warn_ratelimited("%s: dropped over-mtu packet: %d > %d\n",
netdev_vport->dev->name,
packet_length(skb), mtu);
goto error;
}
if (unlikely(skb_warn_if_lro(skb)))
goto error;
skb->dev = netdev_vport->dev;
forward_ip_summed(skb, true);
if (vlan_tx_tag_present(skb) && !dev_supports_vlan_tx(skb->dev)) {
int features;
features = netif_skb_features(skb);
if (!vlan_tso)
features &= ~(NETIF_F_TSO | NETIF_F_TSO6 |
NETIF_F_UFO | NETIF_F_FSO);
if (netif_needs_gso(skb, features)) {
struct sk_buff *nskb;
nskb = skb_gso_segment(skb, features);
if (!nskb) {
if (unlikely(skb_cloned(skb) &&
pskb_expand_head(skb, 0, 0, GFP_ATOMIC))) {
kfree_skb(skb);
return 0;
}
skb_shinfo(skb)->gso_type &= ~SKB_GSO_DODGY;
goto tag;
}
if (IS_ERR(nskb)) {
kfree_skb(skb);
return 0;
}
consume_skb(skb);
skb = nskb;
len = 0;
do {
nskb = skb->next;
skb->next = NULL;
skb = __vlan_put_tag(skb, vlan_tx_tag_get(skb));
if (likely(skb)) {
len += skb->len;
vlan_set_tci(skb, 0);
dev_queue_xmit(skb);
}
skb = nskb;
} while (skb);
return len;
}
tag:
skb = __vlan_put_tag(skb, vlan_tx_tag_get(skb));
if (unlikely(!skb))
return 0;
vlan_set_tci(skb, 0);
}
len = skb->len;
dev_queue_xmit(skb); //before this, all headers should be ready
return len;
error:
kfree_skb(skb);
ovs_vport_record_error(vport, VPORT_E_TX_DROPPED);
return 0;
}
netdev_tx_t
islpci_eth_transmit(struct sk_buff *skb, struct net_device *ndev)
{
islpci_private *priv = netdev_priv(ndev);
isl38xx_control_block *cb = priv->control_block;
u32 index;
dma_addr_t pci_map_address;
int frame_size;
isl38xx_fragment *fragment;
int offset;
struct sk_buff *newskb;
int newskb_offset;
unsigned long flags;
unsigned char wds_mac[6];
u32 curr_frag;
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_FUNCTION_CALLS, "islpci_eth_transmit\n");
#endif
/* lock the driver code */
spin_lock_irqsave(&priv->slock, flags);
/* check whether the destination queue has enough fragments for the frame */
curr_frag = le32_to_cpu(cb->driver_curr_frag[ISL38XX_CB_TX_DATA_LQ]);
if (unlikely(curr_frag - priv->free_data_tx >= ISL38XX_CB_TX_QSIZE)) {
printk(KERN_ERR "%s: transmit device queue full when awake\n",
ndev->name);
netif_stop_queue(ndev);
/* trigger the device */
isl38xx_w32_flush(priv->device_base, ISL38XX_DEV_INT_UPDATE,
ISL38XX_DEV_INT_REG);
udelay(ISL38XX_WRITEIO_DELAY);
goto drop_free;
}
/* Check alignment and WDS frame formatting. The start of the packet should
* be aligned on a 4-byte boundary. If WDS is enabled add another 6 bytes
* and add WDS address information */
if (likely(((long) skb->data & 0x03) | init_wds)) {
/* get the number of bytes to add and re-align */
offset = (4 - (long) skb->data) & 0x03;
offset += init_wds ? 6 : 0;
/* check whether the current skb can be used */
if (!skb_cloned(skb) && (skb_tailroom(skb) >= offset)) {
unsigned char *src = skb->data;
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "skb offset %i wds %i\n", offset,
init_wds);
#endif
/* align the buffer on 4-byte boundary */
skb_reserve(skb, (4 - (long) skb->data) & 0x03);
if (init_wds) {
/* wds requires an additional address field of 6 bytes */
skb_put(skb, 6);
#ifdef ISLPCI_ETH_DEBUG
printk("islpci_eth_transmit:wds_mac\n");
#endif
memmove(skb->data + 6, src, skb->len);
skb_copy_to_linear_data(skb, wds_mac, 6);
} else {
memmove(skb->data, src, skb->len);
}
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "memmove %p %p %i\n", skb->data,
src, skb->len);
#endif
} else {
newskb =
dev_alloc_skb(init_wds ? skb->len + 6 : skb->len);
if (unlikely(newskb == NULL)) {
printk(KERN_ERR "%s: Cannot allocate skb\n",
ndev->name);
goto drop_free;
}
newskb_offset = (4 - (long) newskb->data) & 0x03;
/* Check if newskb->data is aligned */
if (newskb_offset)
skb_reserve(newskb, newskb_offset);
skb_put(newskb, init_wds ? skb->len + 6 : skb->len);
if (init_wds) {
skb_copy_from_linear_data(skb,
newskb->data + 6,
skb->len);
skb_copy_to_linear_data(newskb, wds_mac, 6);
#ifdef ISLPCI_ETH_DEBUG
printk("islpci_eth_transmit:wds_mac\n");
#endif
} else
skb_copy_from_linear_data(skb, newskb->data,
skb->len);
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_TRACING, "memcpy %p %p %i wds %i\n",
newskb->data, skb->data, skb->len, init_wds);
#endif
newskb->dev = skb->dev;
dev_kfree_skb_irq(skb);
skb = newskb;
}
}
/* display the buffer contents for debugging */
#if VERBOSE > SHOW_ERROR_MESSAGES
DEBUG(SHOW_BUFFER_CONTENTS, "\ntx %p ", skb->data);
display_buffer((char *) skb->data, skb->len);
#endif
/* map the skb buffer to pci memory for DMA operation */
pci_map_address = pci_map_single(priv->pdev,
(void *) skb->data, skb->len,
PCI_DMA_TODEVICE);
if (unlikely(pci_map_address == 0)) {
printk(KERN_WARNING "%s: cannot map buffer to PCI\n",
ndev->name);
goto drop_free;
}
/* Place the fragment in the control block structure. */
index = curr_frag % ISL38XX_CB_TX_QSIZE;
fragment = &cb->tx_data_low[index];
priv->pci_map_tx_address[index] = pci_map_address;
/* store the skb address for future freeing */
priv->data_low_tx[index] = skb;
/* set the proper fragment start address and size information */
frame_size = skb->len;
fragment->size = cpu_to_le16(frame_size);
fragment->flags = cpu_to_le16(0); /* set to 1 if more fragments */
fragment->address = cpu_to_le32(pci_map_address);
curr_frag++;
/* The fragment address in the control block must have been
* written before announcing the frame buffer to device. */
wmb();
cb->driver_curr_frag[ISL38XX_CB_TX_DATA_LQ] = cpu_to_le32(curr_frag);
if (curr_frag - priv->free_data_tx + ISL38XX_MIN_QTHRESHOLD
> ISL38XX_CB_TX_QSIZE) {
/* stop sends from upper layers */
netif_stop_queue(ndev);
/* set the full flag for the transmission queue */
priv->data_low_tx_full = 1;
}
ndev->stats.tx_packets++;
ndev->stats.tx_bytes += skb->len;
/* trigger the device */
islpci_trigger(priv);
/* unlock the driver code */
spin_unlock_irqrestore(&priv->slock, flags);
return NETDEV_TX_OK;
drop_free:
ndev->stats.tx_dropped++;
spin_unlock_irqrestore(&priv->slock, flags);
dev_kfree_skb(skb);
return NETDEV_TX_OK;
}
